/**
* Execute this function when the robot is on the ramp
*/
void isOnRamp(){
playImmediateTone(1200, 20);
wait1Msec(200);
// Drive forward
motor[motorD] = 20;
motor[motorF] = 20;
getValues();
while(x < 50){
getValues();
wait1Msec(10);
}
/* x > - 60, so on flat ground */
// And disable them
motor[motorD] = 0;
motor[motorF] = 0;
playImmediateTone(1200, 20);
wait1Msec(200);
}
task main ()
{
// This struct holds all the sensor related data
tDIMC compass;
tSensors DIMC = S1;
// Our local variables
short strength = 0;
// Fire up the compass and initialize it. Only needs to be done once.
if (!initSensor(&compass, DIMC))
playSound(soundException);
// Loop forever, reading the sensor and calulating total
// field strength
while (true)
{
// Read the Compass
if (!readSensor(&compass))
playSound(soundException);
// calculate the field strength
strength = sqrt(pow(compass.axes[0], 2) + pow(compass.axes[1], 2) + pow(compass.axes[2], 2));
// Play a tone of the frequency of the field strength
// Great for annoying the cat/dog/wife/parent
playImmediateTone(strength, 8);
// display on the screen
displayCenteredBigTextLine(3, "%d", strength);
sleep(50);
}
}
task main()
{
int i;
servo[leftEye] = LSERVO_CENTER;
servo[rightEye] = RSERVO_CENTER;
init_path();
add_segment(24, 0, 50); // Move to beacon sensing position.
stop_path();
dead_reckon();
if (SensorValue[carrot] < 60) {
position = 3;
} else if (SensorValue[carrot] < 80) { // If ultrasonic sensor sees a position,
position = 1; // set variable "position" to position number.
} else {
position = 2;
}
move_to_pole(position); // Move to pole using position number.
for (i = 0; i < position; i++) { // Beep for the position number that the
playImmediateTone(251, 50); // ultrasonic sensor sees.
wait1Msec(1000);
}
}
task main()
{
int i;
int center_position;
initialize_robot();
//waitForStart();
// startTask(raise_elbow);
startTask(raise_hand);
startTask(lower_ramp);
// initialize_receiver(irr_left, irr_right);
servo[leftEye] = LSERVO_CENTER;
servo[rightEye] = RSERVO_CENTER;
center_position = ultrasound_mk(carrot, -24, US_DIST_POS_1, US_DIST_POS_2, US_DIST_POS_3);
//startTask(raise_ramp);
if (center_position == -1) {
playImmediateTone(251, 150);
}
for (i = 0; i < center_position; i++) {
playImmediateTone(251, 50);
wait1Msec(1000);
}
nMotorEncoder[elbow] = 0;
motor[elbow] = 20;
while (nMotorEncoder[elbow] <= 3600) {
nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[elbow]);
}
motor[elbow] = 0;
// move_to_pole(center_position);
while(true) {
nxtDisplayCenteredBigTextLine(3, "%d", USreadDist(carrot));
}
}
/**
* Execute this function when the robot is on the ground
*/
void isOnGround(){
playImmediateTone(400, 20);
wait1Msec(200);
motor[motorD] = 100;
motor[motorF] = 100;
wait1Msec(1000);
}
task main()
{
int LY = 0;
int LX = 0;
int RY = 0;
int RX = 0;
int threshold = 15;
bool stopCatapult = false;
//startTask(lineTrackTest);
//stopTask(main);
startTask(log);
while(1)
{
//for deadzones; when the joystick value for an axis is below the threshold, the motors controlled by that joystick will not move in that direction
LY = (abs(vexRT[Ch3]) > threshold) ? vexRT[Ch3] : 0;
LX = (abs(vexRT[Ch4]) > threshold) ? vexRT[Ch4] : 0;
RY = (abs(vexRT[Ch2]) > threshold) ? vexRT[Ch2] : 0;
RX = (abs(vexRT[Ch1]) > threshold) ? vexRT[Ch1] : 0;
motor[lDriveFront] = LY + LX;
motor[lDriveBack] = LY - LX;
motor[rDriveFront] = RY - RX;
motor[rDriveBack] = RY + RX;
if (!vexRT[Btn7D] && !testMode) {
if(vexRT[Btn5U] == 1)
{
setCatapultMotors(127);
}
else if((vexRT[Btn5D] == 1 && !SensorValue[platformAttached]) || (vexRT[Btn5D] && vexRT[Btn8D]))
{
setCatapultMotors(-127);
}
else
{
setCatapultMotors(0);
}
if (vexRT[Btn6U]) {
SensorValue[platformSolenoid] = 1;
}
if (vexRT[Btn6D]) {
SensorValue[platformSolenoid] = 0;
}
if (vexRT[Btn8U]) {
SensorValue[openGate] = 1;
SensorValue[closeGate] = 0;
}
if (vexRT[Btn8D]) {
SensorValue[openGate] = 0;
SensorValue[closeGate] = 1;
}
}
//graph armPot, catapultUp, platformSolenoid, platformAttached v. time
else { //7D is pressed
//startTask(resetCatapult); //set the catapult motors to -127 to release the catapult and then start moving it down again; this is in a separate task so that the catapult will stop when
// it is supposed to even if the catapultUp condition (below) is never met
//play warning tone and wait before starting
playImmediateTone(1100, 100); //duration in 10Msec
wait1Msec(1500);
SensorValue[platformSolenoid] = 0; //make sure platform will be attached
SensorValue[openGate] = 1;
SensorValue[closeGate] = 0;
wait1Msec(500);
setCatapultMotors(-127); //start catapult motors to fire catapult
bool catapultUpTimedOut = false;
int catapultTimeOutTime = 3000; //in milliseconds; wait 3 seconds at most for the catapult to go all the way up
time1[T1] = 0; //start timing how long we've been waiting for the catapult to go all the way up
while(!SensorValue[catapultUp] || SensorValue[catapultPot] < 3700) { //when the catapult is up and the limit switch indicating this is activated, move on
if (time1[T1] > catapultTimeOutTime) {
catapultUpTimedOut = true;
break; //exit this while loop
}
wait1Msec(25);
}
if (!catapultUpTimedOut) { //if catapult up didn't time out
SensorValue[platformSolenoid] = 1; //release platform
//setCatapultMotors(127);
//wait1Msec(125);
time1[T1] = 0;
int catapultResetTimeOutTime = 7000; //check this value
while (!SensorValue[platformAttached] || SensorValue[catapultPot] > 2300) { //when platform is attached and catapult is down, move one
if (time1[T1] > catapultResetTimeOutTime) {
break; //exit this while loop
}
wait1Msec(25);
}
}
//if either wait operation times out, the catapult will stop moving and the platform solenoid will extend out; the variable platform released is not used
setCatapultMotors(0); //stop moving catapult
SensorValue[platformSolenoid] = 0; //reattach platform
platformReleased = false;
}
}
}
