Advertisement
Guest User

Robotc

a guest
Sep 6th, 2016
156
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Robots 3.45 KB | None | 0 0
  1. #pragma config(Sensor, S1,     ,               sensorEV3_Gyro)
  2. #pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, driveLeft, encoder)
  3. #pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, driveRight, encoder)
  4. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  5. void fwd(int a,int d)
  6. {
  7.     int current_gyro;
  8.     nMotorEncoder[B] = 0;
  9.     nMotorEncoder[C] = 0;
  10.     if(a == 0)
  11.     {
  12.         current_gyro = SensorValue(S1);
  13.         while(nMotorEncoder[C] < 100)
  14.         {
  15.             setMotorSync(B, C, current_gyro*-0.7, 10);
  16.             current_gyro = SensorValue(S1);
  17.         }
  18.         while(nMotorEncoder[C] < d)
  19.         {
  20.             setMotorSync(B, C, current_gyro*-0.7, 30);
  21.             current_gyro = SensorValue(S1);
  22.         }
  23.     }
  24.     else if(a == 1)
  25.     {
  26.         current_gyro = SensorValue(S1) - 90;
  27.         while(nMotorEncoder[C] < 100)
  28.         {
  29.             setMotorSync(B, C, current_gyro*-0.7, 10);
  30.             current_gyro = SensorValue(S1) - 90;
  31.         }
  32.         while(nMotorEncoder[C] < d)
  33.         {
  34.             setMotorSync(B, C, current_gyro*-0.7, 30);
  35.             current_gyro = SensorValue(S1) - 90;
  36.         }
  37.     }
  38.     else if(a == 3)
  39.     {
  40.         current_gyro = SensorValue(S1) + 90;
  41.         while(nMotorEncoder[C] < 100)
  42.         {
  43.             setMotorSync(B, C, current_gyro*-0.7, 10);
  44.             current_gyro = SensorValue(S1) + 90;
  45.         }
  46.         while(nMotorEncoder[C] < d)
  47.         {
  48.             setMotorSync(B, C, current_gyro*-0.7, 30);
  49.             current_gyro = SensorValue(S1) + 90;
  50.         }
  51.     }
  52.     motor[B] = 0;
  53.     motor[C] = 0;
  54.     wait1Msec(200);
  55. }
  56. void turnright90(int a)
  57. {
  58.     int current_gyro;
  59.     current_gyro = SensorValue(S1);
  60.     if(a == 0)
  61.     {
  62.         while(current_gyro < 70)
  63.         {
  64.             current_gyro = SensorValue(S1);
  65.             motor[B] = 20;
  66.             motor[C] = -20;
  67.         }
  68.         while(current_gyro < 88)
  69.         {
  70.             current_gyro = SensorValue(S1);
  71.             motor[B] = 10;
  72.             motor[C] = -10;
  73.         }
  74.     }
  75.     else if(a == 3)
  76.     {
  77.         while(current_gyro < -20)
  78.         {
  79.             current_gyro = SensorValue(S1);
  80.             motor[B] = 20;
  81.             motor[C] = -20;
  82.         }
  83.         while(current_gyro < -2)
  84.         {
  85.             current_gyro = SensorValue(S1);
  86.             motor[B] = 10;
  87.             motor[C] = -10;
  88.         }
  89.     }
  90.  
  91.     motor[B] = 0;
  92.     motor[C] = 0;
  93.     wait1Msec(200);
  94. }
  95. void turnleft90(int a)
  96. {
  97.     int current_gyro;
  98.     current_gyro = SensorValue(S1);
  99.     if(a == 0)
  100.     {
  101.         while(current_gyro > -70)
  102.         {
  103.             current_gyro = SensorValue(S1);
  104.             motor[B] = -20;
  105.             motor[C] = 20;
  106.         }
  107.         while(current_gyro > -88)
  108.         {
  109.             current_gyro = SensorValue(S1);
  110.             motor[B] = -10;
  111.             motor[C] = 10;
  112.         }
  113.     }
  114.     else if(a == 1)
  115.     {
  116.         while(current_gyro > 20)
  117.         {
  118.             current_gyro = SensorValue(S1);
  119.             motor[B] = -20;
  120.             motor[C] = 20;
  121.         }
  122.         while(current_gyro > 2)
  123.         {
  124.             current_gyro = SensorValue(S1);
  125.             motor[B] = -10;
  126.             motor[C] = 10;
  127.         }
  128.     }
  129.  
  130.     motor[B] = 0;
  131.     motor[C] = 0;
  132.     wait1Msec(200);
  133. }
  134.  
  135. task main()
  136. {
  137.     resetGyro(S1);
  138.  
  139.  
  140.     fwd(0,650);
  141.     turnright90(0);
  142.  
  143.     fwd(1,700);
  144.     turnleft90(1);
  145.  
  146.     fwd(0,700);
  147.     turnleft90(0);
  148.  
  149.     fwd(3,1300);
  150.     turnright90(3);
  151.  
  152.     fwd(0,650);
  153.     turnright90(0);
  154.  
  155.     nMotorEncoder[B] = 0;
  156.     nMotorEncoder[C] = 0;
  157.     while(nMotorEncoder[C] < 20)
  158.     {
  159.         motor[B] = 0;
  160.         motor[C] = 10;
  161.     }
  162.     motor[B] = 0;
  163.     motor[C] = 0;
  164.     resetGyro(S1);
  165.     wait1Msec(200);
  166.    
  167.    
  168.     fwd(0,2500);
  169.     turnleft90(0);
  170.     resetGyro(S1);
  171.    
  172.     fwd(0,630);
  173.     turnleft90(0);
  174.     fwd(3,3100);
  175.     turnright90(3);
  176.     resetGyro(S1);
  177.    
  178.     fwd(0,600);
  179.     turnright90(0);
  180.     fwd(1,3800);
  181.     turnleft90(1);
  182.     fwd(0,700);
  183.     turnleft90(0);
  184.     resetGyro(S1);
  185.    
  186.     fwd(0,4400);
  187.     turnright90(0);
  188.     resetGyro(S1);
  189.    
  190.     fwd(0,650);
  191.     turnright90(0);
  192.     fwd(1,1850);
  193.     turnleft90(1);
  194.     fwd(0,850);
  195.    
  196.    
  197. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement