SHARE
TWEET

Untitled

a guest Dec 16th, 2018 72 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #pragma config(I2C_Usage, I2C1, i2cSensors)
  2. #pragma config(Sensor, I2C_1,           frtrtdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  3. #pragma config(Sensor, I2C_2,           backrtdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  4. #pragma config(Sensor, I2C_3,           frtlftdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  5. #pragma config(Sensor, I2C_4,           backlftdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  6. #pragma config(Sensor, I2C_5,           toprtarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  7. #pragma config(Sensor, I2C_6,           btlftarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  8. #pragma config(Sensor, I2C_7,           toplftarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
  9. #pragma config(Motor,  port1,           toprtarm,      tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
  10. #pragma config(Motor,  port2,           btrtarm,       tmotorVex393, openLoop, reversed)
  11. #pragma config(Motor,  port3,           frtrtdrive,    tmotorVex393, openLoop, encoder, encoderPort, I2C_1, 1000)
  12. #pragma config(Motor,  port4,           backrtdrive,   tmotorVex393, openLoop, encoder, encoderPort, I2C_2, 1000)
  13. #pragma config(Motor,  port5,           frtlftdrive,   tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_3, 1000)
  14. #pragma config(Motor,  port6,           backlftdrive,  tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_4, 1000)
  15. #pragma config(Motor,  port7,           btlftarm,      tmotorVex393, openLoop, encoder, encoderPort, I2C_6, 1000)
  16. #pragma config(Motor,  port8,           toplftarm,     tmotorVex393, openLoop, encoder, encoderPort, I2C_7, 1000)
  17. #pragma config(Motor,  port9,           lftin,         tmotorVex269, openLoop)
  18. #pragma config(Motor,  port10,          rtin,          tmotorVex269, openLoop, reversed)
  19. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  20.  
  21. #pragma platform(VEX)
  22.  
  23. //Competition Control and Duration Settings
  24. #pragma competitionControl(Competition)
  25. #pragma autonomousDuration(20)
  26. #pragma userControlDuration(120)
  27.  
  28. #include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
  29.  
  30. #define TPR 627.2 //Ticks per revolution
  31.  
  32. /////////////////////////////////////////////////////////////////////////////////////////
  33. //
  34. //                          Pre-Autonomous Functions
  35. //
  36. // You may want to perform some actions before the competition starts. Do them in the
  37. // following function.
  38. //
  39. /////////////////////////////////////////////////////////////////////////////////////////
  40.  
  41. float kp = 0.625;
  42. float ki = 0;
  43. float kd = 0.0;
  44. int previousError = 0;
  45. float errorSum = 0;
  46. int motPower = 0;
  47. void armPID(int target)
  48. {
  49.   int error = target - nMotorEncoder[toprtarme];
  50.   int derivative = error-previousError;
  51.   motPower = (error*kp+errorSum*ki+derivative*kd);
  52.   motor[toprtarm] = motPower;
  53.   motor[toplftarm] = -motPower;
  54.   motor[btrtarm] = motPower;
  55.   motor[btlftarm] = -motPower;
  56.   previousError = error;
  57.   errorSum += error/1024.0;
  58. }
  59. void PIDreset(){
  60.   previousError = 0;
  61.   errorSum = 0;
  62. }
  63. void pre_auton()
  64. {
  65.   PIDreset();
  66. }
  67. void fdrive(int power)
  68. {
  69.     motor[frtrtdrive]=power;
  70.     motor[frtlftdrive]=power;
  71.     motor[backlftdrive]=power;
  72.     motor[backrtdrive]=power;
  73. }
  74. float leftdistance()
  75. {
  76.     return nMotorEncoder[frtlftdrivee] * (TPR/2) * (4*PI);
  77. }
  78. float rightdistance()
  79. {
  80.     return nMotorEncoder[frtrtdrivee] * (TPR/2) * (4*PI);
  81. }
  82. void distancedrive(float inches)
  83. {  
  84.     nMotorEncoder[frtrtdrivee]=0;
  85.     while(true)
  86.     {  
  87.         float dist=abs(leftdistance());
  88.     if (dist<inches)
  89.         fdrive(127);
  90.   else
  91.     fdrive(0);
  92.         break;
  93.   }
  94. }
  95. void turn(float degrees,int direction)//turn; degrees of rotation, and direction of rotation
  96. {
  97.     float inches = (degrees/20)*PI;
  98.   nMotorEncoder[frtrtdrivee]=0;
  99.   nMotorEncoder[frtlftdrivee]=0;
  100.   if(direction==1){
  101.     while(true){
  102.       float leftdist=abs(leftdistance());
  103.         float rightdist=abs(rightdistance());
  104.           if (leftdist<inches){
  105.           //motor[frtrtdrive]=power1;
  106.         //motor[backrtdrive]=power1;
  107.         motor[frtlftdrive]=127;
  108.             motor[backlftdrive]=127;
  109.           }
  110.           if(rightdist<inches){
  111.             motor[frtrtdrive]=-127;
  112.             motor[backrtdrive]=-127;
  113.           }
  114.           else{
  115.             fdrive(0);
  116.           }
  117.       }
  118.   }
  119.   if(direction==0){
  120.       while(true)
  121.     {
  122.       float leftdist=abs(leftdistance());
  123.         float rightdist=abs(rightdistance());
  124.         if (leftdist<inches){
  125.           //motor[frtrtdrive]=power1;
  126.         //motor[backrtdrive]=power1;
  127.         motor[frtlftdrive]=-127;
  128.             motor[backlftdrive]=-127;
  129.           }
  130.         if(rightdist<inches){
  131.           motor[frtrtdrive]=127;
  132.           motor[backrtdrive]=127;
  133.         }
  134.         else{
  135.           fdrive(0);
  136.         }
  137.     }
  138.   }
  139. }
  140. void intake(int power)
  141. {
  142.     motor[rtin]=power;
  143.     motor[lftin]=power;
  144. }
  145.  
  146. /////////////////////////////////////////////////////////////////////////////////////////
  147. //
  148. //                                 Autonomous Task
  149. //
  150. // This task is used to control your robot during the autonomous phase of a VEX Competition.
  151. // You must modify the code to add your own robot specific commands here.
  152. //
  153. /////////////////////////////////////////////////////////////////////////////////////////
  154.  
  155. task autonomous()
  156. {
  157.     PIDreset();
  158. }
  159.  
  160. /////////////////////////////////////////////////////////////////////////////////////////
  161. //
  162. //                                 User Control Task
  163. //
  164. // This task is used to control your robot during the user control phase of a VEX Competition.
  165. // You must modify the code to add your own robot specific commands here.
  166. //
  167. /////////////////////////////////////////////////////////////////////////////////////////
  168.  
  169. task usercontrol()
  170. {
  171.     // User control code here, inside the loop
  172.  
  173.     while (true)
  174.     {
  175.       //The first four statements are for the drivebase.
  176.         //The up and down controls on the left and right joysticks control the drive motors.
  177.         //Moving the left joystick forward will make the left motors go forward at full power (Same for right side).
  178.         //Thus turning the right joystick forward and the left joystick backward will turn the robot to the left.
  179.         //*Needs confirmation* Turning the right joystick halfway forward SHOULD make the right motors move at half power.
  180.         motor[frtrtdrive]=vexRT[Ch2];
  181.         motor[backrtdrive]=vexRT[Ch2];
  182.         motor[frtlftdrive]=vexRT[Ch3];
  183.         motor[backlftdrive]=vexRT[Ch3];
  184.         //The next two "if" are for the lift.
  185.         //The right shoulder buttons will move the lift up and down at full power.
  186.         //The closer button will move the lift up, and the further back button will move the lift down.
  187.         if (vexRT[Btn6U]==1)
  188.         {
  189.           motor(toplftarm)=127;
  190.           motor(btrtarm)=127;
  191.           motor(toprtarm)=127;
  192.           motor(btlftarm)=127;
  193.       }
  194.         if (vexRT[Btn6D]==1)
  195.         {
  196.           motor(toplftarm)=-127;
  197.           motor(btrtarm)=-127;
  198.           motor(toprtarm)=-127;
  199.           motor(btlftarm)=-127;
  200.         }
  201.         if (vexRT[Btn6D]==0&&vexRT[Btn6U]==0)
  202.         {
  203.           motor(toplftarm)=0;
  204.           motor(btrtarm)=0;
  205.           motor(toprtarm)=0;
  206.           motor(btlftarm)=0;
  207.         }
  208.  
  209.         //The final two "if" statements are for controlling  intake.
  210.         //The left shoulder buttons will turn the intake.
  211.         //The closer left button will make the robot intake, and the button further back will make the robot outtake.
  212.         if (vexRT[Btn5U]==1)
  213.         {
  214.           motor(rtin)=127;
  215.           motor(lftin)=127;
  216.         }
  217.         if (vexRT[Btn5U]==0&&vexRT[Btn5D]==0){
  218.           motor(rtin)=0;
  219.           motor(lftin)=0;
  220.         }
  221.         if (vexRT[Btn5D]==1)
  222.         {
  223.           motor(rtin)=-127;
  224.           motor(lftin)=-127;
  225.         }
  226.     }
  227. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top