 | Code: #pragma config(Hubs, S1, HTMotor, none, none, none) #pragma config(Hubs, S2, HTMotor, HTServo, none, none) #pragma config(Sensor, S2, compass, sensorNone) #pragma config(Sensor, S3, light, sensorLightActive) #pragma config(Sensor, S4, sonar, sensorSONAR) #pragma config(Motor, mtr_S1_C1_1, motorA, tmotorTetrix, PIDControl) #pragma config(Motor, mtr_S1_C1_2, motorB, tmotorTetrix, PIDControl) #pragma config(Motor, mtr_S2_C1_1, motorC, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S2_C1_2, motorD, tmotorTetrix, openLoop) #pragma config(Servo, srvo_S2_C2_1, armServo1, tServoStandard) #pragma config(Servo, srvo_S2_C2_2, armServo2, tServoStandard) #pragma config(Servo, srvo_S2_C2_3, clawServo, tServoStandard) #pragma config(Servo, srvo_S2_C2_4, servo4, tServoNone) #pragma config(Servo, srvo_S2_C2_5, servo5, tServoNone) #pragma config(Servo, srvo_S2_C2_6, servo6, tServoNone) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#define DEADBAND_THRESHOLD 10 //*!!Define as 10*// #define BOUND(x) (((x) > 100)? 100: ((x) < -100)? -100: (x)) //*!!Makes sure that the joystick is between -100 and 100 !!*// #define DEADBAND(x) ((abs(x) >= DEADBAND_THRESHOLD)? x: 0) //*!!If the joystick is between -10 and 10, it will be treated as 0 !!*//
#define TIME_INTERVAL 100
void arm_servos( int joy2_y1 ) { int k = 127; if ( DEADBAND(joy2_y1) > 0 ) { servo[armServo1] = k; servo[armServo2] = k; } else { servo[armServo1] = 0; servo[armServo2] = 0; } }
void claw_servo ( int btn2, int btn4 ) { int k = 127; if ( btn2 == 1 ) { servo[ clawServo ] = k; } else if ( btn4 == 1 ) { servo[ clawServo ] = 0; } }
#include "JoystickDriver.c" task main() { while(true) { getJoystickSettings(joystick); // Update Buttons and Joysticks
arm_servos( joystick.joy2_y1 ); claw_servo( joy2Btn(2), joy2Btn(4) ); wait1Msec(TIME_INTERVAL); } }
|  |