 | Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Motor, motorA, lmotorA, tmotorNormal, openLoop, reversed)
#pragma config(Motor, motorB, lmotorB, tmotorNormal, openLoop, reversed)
#pragma config(Motor, motorC, loader, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_1, dmotorA, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, dmotorB, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_1, combine, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_2, basketmo, tmotorNormal, openLoop,)
#pragma config(Servo, srvo_S1_C3_1, aim, tServoNormal)
#pragma config(Servo, srvo_S1_C3_2, ramp, tServoNormal)
#pragma config(Servo, srvo_S1_C3_3, flpusher, tServoNormal)
#pragma config(Servo, srvo_S1_C3_4, rlpusher, tServoNormal)
#pragma config(Servo, srvo_S1_C3_5, basket, tServoNormal)
#pragma config(Servo, srvo_S1_C3_6, , tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//JoystickDreiver.c is the magic behind the getJoytickSettings(), and a few other functions;
#include "JoystickDriver.c"
/*if(spunup == false)
{
if(LLAUNCHER_KEY)
buttonWasPressed = true;
else if(!LLAUNCHER_KEY && buttonWasPressed)
{
loaderMotor(100);
timer = nPgmTime;
buttonWasPressed = false;
spunup = true;
}
}
else if(LLAUNCHER_KEY)
buttonWasPressed = true;
else if(!LLAUNCHER_KEY && buttonWasPressed)
{
launchMotors(0,0);
loaderMotor(0);
buttonWasPressed = false;
spunup = false;
}*/
//Keymap
//Joystick 1
#define RAMP_KEY joy1Btn(5)
#define COMBINE_KEY joy1Btn(6)
//Joystick 2
#define BASKETWEAKSERVO_UP_KEY joy2Btn(6)
#define BASKETWEAKSERVO_DOWN_KEY joy2Btn(8)
#define BASKETSERVO_UP_KEY joy2Btn(3)
#define BASKETSERVO_DOWN_KEY joy2Btn(1)
#define LOADER_REVERSE_KEY joy2Btn(4)
#define FLPUSHER_KEY joy2Btn(5)
#define LOADER_KEY joy2Btn(2)
#define RLPUSHER_KEY joy2Btn(7)
//Other misc. definitions
#define TELEOP_TIME 900
#define CONTROLLER_DEAD_ZONE 20
//Declare Functions For Project
//Init is where we'll put anything that needs to be done before the round starts. EX Put servos in a certain position,
//etc.
void init();
//drive is for the teleop commands
void drive();
// *Motors() is shorthand for motor[lmotorA] = ... etc
void launchMotors(short m1, short m2);
void driveMotors(short m1, short m2);
void combineMotor(short m);
void loaderMotor(short m);
void basketMotor(short m);
void init()
{
servo[ramp] = 114;
servo[aim] = 42; //Oh man
servo[flpusher] = 100;
servo[rlpusher] = 166;
}
task main()
{
init();
waitForStart();
drive();
}
void drive()
{
//Variables for toggle switches
bool combineWasPressed = false;
bool combineSpunup = false;
bool basketupWasPressed = false;
bool basketdownWasPressed = false;
bool rampWasPressed = false;
bool rampSpunup = false;
int encoderA, encoderB;
int basketServoPosition = servo[basket];
ClearTimer(T1);
launchMotors(100,100);
while(/*time100[T1] <= TELEOP_TIME*/1)
{
encoderA = nMotorEncoder[dmotorA];
encoderB = nMotorEncoder[dmotorB];
getJoystickSettings(joystick);
//Launcher/Loader Controls
if(LOADER_KEY || LOADER_REVERSE_KEY)
{
if(LOADER_KEY)
loaderMotor(100);
if(LOADER_REVERSE_KEY)
loaderMotor(-100);
}
else
loaderMotor(0);
//Combine Controls
if(combineSpunup == false)
{
if(COMBINE_KEY)
combineWasPressed = true;
else if(!COMBINE_KEY && combineWasPressed)
{
combineMotor(100);
combineWasPressed = false;
combineSpunup = true;
}
}
else if(COMBINE_KEY)
combineWasPressed = true;
else if(!COMBINE_KEY && combineWasPressed)
{
combineMotor(0);
combineWasPressed = false;
combineSpunup = false;
}
//Basket Motor
if(joystick.joy2_y1 > 0)
basketMotor(joystick.joy2_y1/4);
else
basketMotor(joystick.joy2_y1/6);
//Basket Servo
if(BASKETWEAKSERVO_UP_KEY)
basketupWasPressed = true;
else if(!BASKETWEAKSERVO_UP_KEY && basketupWasPressed)
{
basketServoPosition += 2;
basketupWasPressed = false;
}
if(BASKETWEAKSERVO_DOWN_KEY)
basketdownWasPressed = true;
else if(!BASKETWEAKSERVO_DOWN_KEY && basketdownWasPressed)
{
basketServoPosition -= 2;
basketdownWasPressed = false;
}
if(BASKETSERVO_UP_KEY)
basketServoPosition = 134;
else if(BASKETSERVO_DOWN_KEY)
basketServoPosition = 86;
servo[basket] = basketServoPosition;
//Ramp Controls
if(rampSpunup == false)
{
if(RAMP_KEY)
rampWasPressed = true;
else if(!RAMP_KEY && rampWasPressed)
{
servo[ramp] = 0;
rampWasPressed = false;
rampSpunup = true;
}
}
else if(RAMP_KEY)
rampWasPressed = true;
else if(!RAMP_KEY && rampWasPressed)
{
servo[ramp] = 120;
rampWasPressed = false;
rampSpunup = false;
}
//Front T-Pusher Controls
if(FLPUSHER_KEY)
servo[flpusher] = 230;
else
servo[flpusher] = 100;
//Rear T-Pusher Controls
if(RLPUSHER_KEY)
servo[rlpusher] = 100;
else
servo[rlpusher] = 166;
//Vroom Vroom
driveMotors(joystick.joy1_y1, joystick.joy1_y2,);
}
}
void launchMotors(short m1, short m2)
{
motor[lmotorA] = m1;
motor[lmotorB] = m2;
}
//Note that driveMotors will ignore values < CONTROLLER_DEAD_ZONE
void driveMotors(short m1, short m2)
{
//Here we scale the joystick control values from (-127,127) to (-100,100) which is what the controller accepts.
//There is also a deadzone of 10 either way to keep it stable. Due to an odd hardware glitch, we also have to
//keep the motors at 1 power in order to keep them from misfiring
if(abs(m1) >= CONTROLLER_DEAD_ZONE)
motor[dmotorA] = m1/1.27;
else
motor[dmotorA] = 1;
if(abs(m2) >= CONTROLLER_DEAD_ZONE)
motor[dmotorB] = m2/1.27;
else
motor[dmotorB] = 1;
}
void combineMotor(short m)
{
motor[combine] = m;
}
void loaderMotor(short m)
{
motor[loader] = m;
}
void basketMotor(short m)
{
motor[basketmo] = m;
}
| |
