 | Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo) #pragma config(Sensor, S1, , sensorI2CMuxController) #pragma config(Sensor, S3, , sensorSONAR) #pragma config(Sensor, S4, , sensorHiTechnicIRSeeker1200) #pragma config(Motor, mtr_S1_C1_1, lwheel, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C1_2, rwheel, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C2_1, flag, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C2_2, blockarm, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C3_1, blockarm2, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C3_2, blocksweeper, tmotorTetrix, openLoop) #pragma config(Servo, srvo_S1_C4_1, flagarm, tServoStandard) #pragma config(Servo, srvo_S1_C4_2, autoblock, tServoStandard) #pragma config(Servo, srvo_S1_C4_3, bucketdoor, tServoStandard) #pragma config(Servo, srvo_S1_C4_4, liftrobo1, tServoStandard) #pragma config(Servo, srvo_S1_C4_5, liftrobo2, tServoStandard) #pragma config(Servo, srvo_S1_C4_6, servo6, tServoNone) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
void initializeRobot() { servo[flagarm] = 25; }
task main() { int threshold = 10; float k = 0.78; bool flagBool = false;
while(true) { getJoystickSettings(joystick);
// turn on right drive motor (1)
if(abs(joystick.joy1_y1) > threshold || abs(joystick.joy1_y1) > abs(joystick.joy2_y1)) { motor[lwheel] = k * joystick.joy1_y1; } else { motor[lwheel] = 0; }
// turn on left drive motor (2)
if(abs(joystick.joy1_y2) > threshold || abs(joystick.joy1_y1) > abs(joystick.joy2_y1)) { motor[rwheel] = -k * joystick.joy1_y2; } else { motor[rwheel] = 0; }
//flag raiser
if(joy1Btn(3)) // extend the flag spinner { motor(flag) = -100; } else { motor(flag) = 0; } if(joy1Btn(4)) { motor(flag) = 100; } else { motor(flag) = 0; }
//get blocks // if()
//pulley raiser
if(joy1Btn(6)) { motor[blockarm] = 100; motor[blockarm2] = -100; } else if(joy1Btn(5)) { motor[blockarm] = -35; motor[blockarm2] = 35; } else { motor[blockarm] = 0; motor[blockarm2] = 0; }
if(joy1Btn(7)) { motor[blocksweeper] = 100; } else { motor[blocksweeper] = 0; } if(joy1Btn(8)) { motor[blocksweeper] = -75; } else { motor[blocksweeper] = 0; }
//activate flag raiser
while(joy1Btn(1)) { if(!flagBool) { servo[flagarm] = 255; } if(flagBool) { servo[flagarm] = 25; } } flagBool = !flagBool; wait10Msec(10); }
//Joystick 2
if(abs(joystick.joy2_y1) > threshold || abs(joystick.joy2_y1) > abs(joystick.joy1_y1)) { motor[lwheel] = k * joystick.joy2_y1; } else { motor[lwheel] = 0; }
// turn on left drive motor (2)
if(abs(joystick.joy2_y2) > threshold || abs(joystick.joy2_y2) > abs(joystick.joy1_y2)) { motor[rwheel] = -k * joystick.joy2_y2; } else { motor[rwheel] = 0; }
if(joy1Btn(3)) { motor(flag) = -100; } if(joy1Btn(4)) { motor(flag) = 100; }
if(joy2Btn(6)) { motor[blockarm] = 100; motor[blockarm2] = -100; } else if(joy2Btn(5)) { motor[blockarm] = -35; motor[blockarm2] = 35; } else { motor[blockarm] = 0; motor[blockarm2] = 0; } if(joy2Btn(1)) { servo[bucketdoor] = 255; } else { servo[bucketdoor] = 0; } if(joy2Btn(2)) { servo[bucketdoor] = 0; } else { servo[bucketdoor] = 255; } if(joy2Btn(7)) { servo[liftrobo1] = 255; servo[liftrobo2] = 255; } if(joy2Btn(8)) { servo[liftrobo1] = 0; servo[liftrobo2] = 0; } } |  |