Skip to content

Instantly share code, notes, and snippets.

@kylestev
Created May 30, 2012 17:00
Show Gist options
  • Save kylestev/2837630 to your computer and use it in GitHub Desktop.
Save kylestev/2837630 to your computer and use it in GitHub Desktop.
Program used for my high school team's robot
#pragma config(Hubs, S4, HTMotor, none, none, none)
#pragma config(Sensor, S2, sonar, sensorSONAR)
#pragma config(Sensor, S3, touch, sensorTouch)
#pragma config(Motor, mtr_S4_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S4_C1_2, motorE, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void move(int motorDPower, int motorEPower, int millis) {
motor[motorD] = motorDPower;
motor[motorE] = motorEPower;
wait1Msec(millis);
}
void stopMotors() {
motor[motorD] = 0;
motor[motorE] = 0;
}
task main() {
int distance = 0;
bool touching = false;
while (true) {
touching = SensorValue[touch] == 1;
distance = SensorValue[sonar];
if (touching) { // if there is an object on the left side of the robot
move(10, -10, 300); // turn to the right so we aren't touching it any more
move(10, 10, 100); // go forward a bit
stopMotors(); // halt the engines
} else if (distance < 20) {
// if there is an object within 20cm of the center of the robot
} else {
// not touching and no objects within 20cm
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment