Skip to content

Instantly share code, notes, and snippets.

@zzggbb
Last active August 29, 2015 14:20
Show Gist options
  • Select an option

  • Save zzggbb/a1605da303544b801e18 to your computer and use it in GitHub Desktop.

Select an option

Save zzggbb/a1605da303544b801e18 to your computer and use it in GitHub Desktop.
#pragma config(Motor, port2, throw, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port7, rmotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port9, lmotor, tmotorServoContinuousRotation, openLoop)
#define MAX_SENS 5
#define THROW_SPEED -127
int speed_axis = 0, steer_axis = 0;
int bound(int n) {
return (n > 127 ? 127 : (n < -127 ? -127 : n));
}
task update_axes() {
speed_axis = vexRT[Ch3];
steer_axis = vexRT[Ch1];
}
task update_motors() {
bool reverse = speed_axis < 0;
int sum = bound(speed_axis + steer_axis);
int diff = bound(speed_axis - steer_axis);
motor[lmotor] = (reverse ? diff : sum);
motor[rmotor] = (reverse ? sum : diff) * -1;
}
task launch() {
motor[throw] = THROW_SPEED;
waitInMilliseconds(100);
motor[throw] = THROW_SPEED * -1;
waitInMilliseconds(100);
motor[throw] = 0;
}
task update_remote() {
if(vexRT[Btn5U] == 1) { startTask(launch); }
}
task main(){
for(;;){
startTask(update_axes);
startTask(update_motors);
startTask(update_remote);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment