Uvic Engr120 Robot Code
Uploaded by
DannyDucHuynhUvic Engr120 Robot Code
Uploaded by
DannyDucHuynh#pragma config(Sensor, in1,
IRmid,
sensorAnalog)
#pragma config(Sensor, in2,
IR1,
sensorAnalog)
#pragma config(Sensor, in7,
IR2,
sensorAnalog)
#pragma config(Sensor, dgtl1, wallsensor,
sensorSONAR_cm)
#pragma config(Sensor, dgtl3, leftbutton,
sensorTouch)
#pragma config(Sensor, dgtl4, rightbutton,
sensorTouch)
#pragma config(Sensor, dgtl5, Light1,
sensorDigitalOut)
#pragma config(Sensor, dgtl6, Light2,
sensorDigitalOut)
#pragma config(Sensor, dgtl8, leftlimit,
sensorTouch)
#pragma config(Sensor, dgtl9, rightlimit,
sensorTouch)
#pragma config(Motor, port1,
motor1,
tmotorVex393_HBridge, ope
nLoop)
#pragma config(Motor, port2,
motor3,
tmotorVex393_MC29, openLo
op, encoderPort, I2C_1)
#pragma config(Motor, port10,
motor2,
tmotorVex393_HBridge, ope
nLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard
!!*//
const int Distance=31;
on test
const int Offset=400;
//Change this for the stop distance depending
//Change this for IR offset depending on test
int max;bool left_pushed;bool right_pushed;
//Monitors the input into the two control buttons
void monitorInput() {
if(SensorValue(leftbutton) && !left_pushed) {
left_pushed = true;
}
if(SensorValue(rightbutton) & !right_pushed) {
right_pushed = true;
}
}
//Will bring all motors to a complete stop
void stop() {
motor[motor1]=0;
motor[motor2]=0;
motor[motor3]=0;
wait1Msec(500);
}
void right() {
motor[motor1]=-40;
motor[motor2]=-40;
}
void forward() {
motor[motor1]=-50;
motor[motor2]=50;
}
//Turns LEDs on
void LEDON(int led) {
SensorValue(Light1)=0;
SensorValue(Light2)=0;
if (led==1)
SensorValue(Light1)=1;
if (led==2)
{SensorValue(Light2)=1;}
}
bool source() {
if (SensorValue[IRmid]>max)
max=SensorValue[IRmid];
if (SensorValue[IR1]>max)
max=SensorValue[IR1];
if (SensorValue[IR2]>max)
max=SensorValue[IR2];
if ((SensorValue[IRmid]>SensorValue[IR1]+Offset+SensorValue[wallsensor])&&(S
ensorValue[IRmid]>SensorValue[IR2]+Offset+SensorValue[wallsensor]))
return true;
else
return false;
}
void move(int x)
{
while(SensorValue[wallsensor] > Distance+x || SensorValue[wallsensor] < 0){
//If the limit switch is pressed, turn the opposite direction of the side the sw
itch is on
if (SensorValue[leftlimit]){
motor[motor1] = 30;
motor[motor2] = -30;
wait1Msec(500);
motor[motor1] = 50;
motor[motor2] = 50;
wait1Msec(300);
motor[motor1] = -30;
motor[motor1] = 30;
}
if (SensorValue[rightlimit]) {
motor[motor1] = 30;
motor[motor2] = -30;
wait1Msec(500);
motor[motor1] = -50;
motor[motor2] = -50;
wait1Msec(300);
motor[motor1] = -30;
motor[motor1] = 30;
}
forward();
//emergency shutdown
if (SensorValue(rightbutton)) {
stop();
break;
}
}
}
void lift() {
//motor[motor3]=30;
//wait1Msec(600);
//motor[motor3]=0;
motor[motor3]=-30;
wait1Msec(800);
motor[motor3]=0;
}
void drop() {
motor[motor3]=30;
wait1Msec(400);
motor[motor3]=0;
motor[motor3]=-30;
wait1Msec(600);
motor[motor3]=0;
}
// Initialize the robot with the left button
task main() {
left_pushed=false;
right_pushed=false;
while (1){
monitorInput();
if (left_pushed){
max=0;
//Detect the Infrared source
while (!source()) {
right();
wait1Msec(50);
}
stop();
//Move toward the source with collision detection
while( SensorValue[wallsensor] > Distance ) {
move(6);
//emergency shutdown
if (SensorValue(rightbutton)) {
stop();
break;
}
}
stop();
//Check again and try to get middle sensor facing highest va
lue of source
while (SensorValue[IRmid]!=(max+SensorValue[wallsensor])) {
right();
wait1Msec(50) ;
}
stop();
//Move toward the source with collision detection
while(SensorValue[wallsensor]>Distance ){
move(0);
//emergency shutdown
if (SensorValue(rightbutton)) {
stop();
break;
}
}
stop();
//Lift the object
lift();
//Move to a near wall and drop
//backward
motor[motor1]=50;
motor[motor2]=-50;
wait1Msec(300);
right();
wait1Msec(1000);
while(((SensorValue[wallsensor]>=100)&&(!SensorValue(rightbutton
)))||source()) {
right();
stop();
move(-25);
stop();
}
//Drop the object
drop();
motor[motor1]=50;
motor[motor2]=-50;
wait1Msec(400);
stop();
//Finished
while ((SensorValue[rightbutton]==0)&&(SensorValue[leftbutton]==
0)) {
LEDON(1);
wait1Msec(200);
LEDON(2);
wait1Msec(200);
}
}
}
}
