Code: #pragma config(Sensor, dgtl1, rightEncoder, sensorQuadEncoder) #pragma config(Sensor, dgtl3, leftEncoder, sensorQuadEncoder) #pragma config(Sensor, dgtl8, sonarSensor, sensorSONAR_inch) #pragma config(Motor, port1, rightMotor, tmotorVex269, openLoop, reversed) #pragma config(Motor, port10, leftMotor, tmotorVex269, openLoop, reversed) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//VOID STATEMENTS =================================================================================
//STRAIGHT void moveStraight(int encoderCount) { SensorValue[rightEncoder] = 0; SensorValue[leftEncoder] = 0;
while(SensorValue[rightEncoder] < encoderCount) { if(SensorValue[leftEncoder] > SensorValue[rightEncoder]) { motor[rightMotor] = 63; motor[leftMotor] = 47; } if(SensorValue[rightEncoder] > SensorValue[leftEncoder]) { motor[rightMotor] = 55; motor[leftMotor] = 63; } if(SensorValue[leftEncoder] == SensorValue[rightEncoder]) { motor[rightMotor] = 63; motor[leftMotor] = 48; } } }
//RIGHT
void moveRight(int encoderCount) { SensorValue[rightEncoder] = 0; SensorValue[leftEncoder] = 0;
while(SensorValue[leftEncoder] < encoderCount) //475!!!!!! { motor[rightMotor] = -63; motor[leftMotor] = 60; }
motor[leftMotor] = 0; motor[rightMotor] = 0; wait1Msec(500); }
task main() {
SensorValue[rightEncoder] = 0; //Reset encoders to zero SensorValue[leftEncoder] = 0;
while(1 == 1) //while true { if(SensorValue[sonarSensor] > 7) // when the robot is greater than 7cm away from an object { moveStraight(1225); //straight for the encoder count 1225 then turn right for the encoder count 450
moveRight(450); } else // if it is closer than 7cm { motor[leftMotor] = 0; //stop motor[rightMotor] = 0; } } }
|