Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, S1, , sensorEV3_Gyro)
- #pragma config(Motor, motorB, B, tmotorEV3_Large, PIDControl, driveLeft, encoder)
- #pragma config(Motor, motorC, C, tmotorEV3_Large, PIDControl, driveRight, encoder)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- void fwd(int a,int d)
- {
- int current_gyro;
- nMotorEncoder[B] = 0;
- nMotorEncoder[C] = 0;
- if(a == 0)
- {
- current_gyro = SensorValue(S1);
- while(nMotorEncoder[C] < 100)
- {
- setMotorSync(B, C, current_gyro*-0.7, 10);
- current_gyro = SensorValue(S1);
- }
- while(nMotorEncoder[C] < d)
- {
- setMotorSync(B, C, current_gyro*-0.7, 30);
- current_gyro = SensorValue(S1);
- }
- }
- else if(a == 1)
- {
- current_gyro = SensorValue(S1) - 90;
- while(nMotorEncoder[C] < 100)
- {
- setMotorSync(B, C, current_gyro*-0.7, 10);
- current_gyro = SensorValue(S1) - 90;
- }
- while(nMotorEncoder[C] < d)
- {
- setMotorSync(B, C, current_gyro*-0.7, 30);
- current_gyro = SensorValue(S1) - 90;
- }
- }
- else if(a == 3)
- {
- current_gyro = SensorValue(S1) + 90;
- while(nMotorEncoder[C] < 100)
- {
- setMotorSync(B, C, current_gyro*-0.7, 10);
- current_gyro = SensorValue(S1) + 90;
- }
- while(nMotorEncoder[C] < d)
- {
- setMotorSync(B, C, current_gyro*-0.7, 30);
- current_gyro = SensorValue(S1) + 90;
- }
- }
- motor[B] = 0;
- motor[C] = 0;
- wait1Msec(200);
- }
- void turnright90(int a)
- {
- int current_gyro;
- current_gyro = SensorValue(S1);
- if(a == 0)
- {
- while(current_gyro < 70)
- {
- current_gyro = SensorValue(S1);
- motor[B] = 20;
- motor[C] = -20;
- }
- while(current_gyro < 88)
- {
- current_gyro = SensorValue(S1);
- motor[B] = 10;
- motor[C] = -10;
- }
- }
- else if(a == 3)
- {
- while(current_gyro < -20)
- {
- current_gyro = SensorValue(S1);
- motor[B] = 20;
- motor[C] = -20;
- }
- while(current_gyro < -2)
- {
- current_gyro = SensorValue(S1);
- motor[B] = 10;
- motor[C] = -10;
- }
- }
- motor[B] = 0;
- motor[C] = 0;
- wait1Msec(200);
- }
- void turnleft90(int a)
- {
- int current_gyro;
- current_gyro = SensorValue(S1);
- if(a == 0)
- {
- while(current_gyro > -70)
- {
- current_gyro = SensorValue(S1);
- motor[B] = -20;
- motor[C] = 20;
- }
- while(current_gyro > -88)
- {
- current_gyro = SensorValue(S1);
- motor[B] = -10;
- motor[C] = 10;
- }
- }
- else if(a == 1)
- {
- while(current_gyro > 20)
- {
- current_gyro = SensorValue(S1);
- motor[B] = -20;
- motor[C] = 20;
- }
- while(current_gyro > 2)
- {
- current_gyro = SensorValue(S1);
- motor[B] = -10;
- motor[C] = 10;
- }
- }
- motor[B] = 0;
- motor[C] = 0;
- wait1Msec(200);
- }
- task main()
- {
- resetGyro(S1);
- fwd(0,650);
- turnright90(0);
- fwd(1,700);
- turnleft90(1);
- fwd(0,700);
- turnleft90(0);
- fwd(3,1300);
- turnright90(3);
- fwd(0,650);
- turnright90(0);
- nMotorEncoder[B] = 0;
- nMotorEncoder[C] = 0;
- while(nMotorEncoder[C] < 20)
- {
- motor[B] = 0;
- motor[C] = 10;
- }
- motor[B] = 0;
- motor[C] = 0;
- resetGyro(S1);
- wait1Msec(200);
- fwd(0,2500);
- turnleft90(0);
- resetGyro(S1);
- fwd(0,630);
- turnleft90(0);
- fwd(3,3100);
- turnright90(3);
- resetGyro(S1);
- fwd(0,600);
- turnright90(0);
- fwd(1,3800);
- turnleft90(1);
- fwd(0,700);
- turnleft90(0);
- resetGyro(S1);
- fwd(0,4400);
- turnright90(0);
- resetGyro(S1);
- fwd(0,650);
- turnright90(0);
- fwd(1,1850);
- turnleft90(1);
- fwd(0,850);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement