API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 18th, 2017
88
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.23 KB | None | 0 0
raw download clone embed print report
  1. #pragma config(I2C_Usage, I2C1, i2cSensors)
  2. #pragma config(Sensor, in1, clawPot2, sensorPotentiometer)
  3. #pragma config(Sensor, in2, clawPot1, sensorPotentiometer)
  4. #pragma config(Sensor, dgtl1, liftBottom, sensorTouch)
  5. #pragma config(Sensor, dgtl2, autonRL, sensorTouch)
  6. #pragma config(Sensor, I2C_1, driveR, sensorQuadEncoderOnI2CPort, , AutoAssign )
  7. #pragma config(Sensor, I2C_2, driveL, sensorQuadEncoderOnI2CPort, , AutoAssign )
  8. #pragma config(Sensor, I2C_3, liftEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign )
  9. #pragma config(Motor, port1, liftL3, tmotorVex393HighSpeed_HBridge, openLoop)
  10. #pragma config(Motor, port2, leftDrive, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
  11. #pragma config(Motor, port3, rightDrive, tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_2)
  12. #pragma config(Motor, port4, intake2, tmotorVex393HighSpeed_MC29, openLoop, reversed)
  13. #pragma config(Motor, port5, intake1, tmotorVex393HighSpeed_MC29, openLoop)
  14. #pragma config(Motor, port6, liftL2, tmotorVex393HighSpeed_MC29, openLoop)
  15. #pragma config(Motor, port7, liftL1, tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_3)
  16. #pragma config(Motor, port8, liftR2, tmotorVex393HighSpeed_MC29, openLoop, driveRight)
  17. #pragma config(Motor, port9, liftR1, tmotorVex393HighSpeed_MC29, openLoop)
  18. #pragma config(Motor, port10, liftR3, tmotorVex393HighSpeed_HBridge, openLoop, reversed)
  19. //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
  20.  
  21. // This code is for the VEX cortex platform
  22. #pragma platform(VEX2)
  23.  
  24. // Select Download method as "competition"
  25. #pragma competitionControl(Competition)
  26.  
  27. //Main competition background code...do not modify!
  28. #include "Vex_Competition_Includes.c"
  29. float desiredClaw1 = 0;
  30. float desiredClaw2 = 0;
  31. float desiredLift = 3340;
  32.  
  33. void lift(int val){
  34. motor[liftL1] = motor[liftL2] =motor[liftL3] = motor[liftR1] = motor[liftR2] = motor[liftR3] = val;
  35. }
  36.  
  37. task driveControl(){
  38. while(true){
  39. motor[leftDrive] = abs(vexRT[Ch3])<10?0:vexRT[Ch3];
  40. motor[rightDrive] = abs(vexRT[Ch2])<10?0:vexRT[Ch2];
  41. }
  42. }
  43.  
  44. int limit(int val, int min = -127, int max = 127){
  45. if(val>max){
  46. return max;
  47. }
  48. if(val<min){
  49. return min;
  50. }
  51. return val;
  52. }
  53.  
  54. task liftControl(){
  55. bool liftMoving = false;
  56. float kP = 0.7;
  57. float kI = 0.01;
  58. float kD = 0.7;
  59. float error;
  60. float integral;
  61. float derivative;
  62. float lastError;
  63. float lastSensorValue;
  64. desiredLift = SensorValue[liftEncoder];
  65.  
  66. while(true) {
  67. if(vexRT[Btn5U]) {
  68. liftMoving = true;
  69. desiredLift -= 30;
  70. }else if (vexRT[Btn5D]) {
  71. liftMoving = true;
  72. if(SensorValue[liftBottom])
  73. desiredLift = SensorValue[liftEncoder];
  74. else
  75. desiredLift += 30;
  76. } else if(liftMoving) {
  77. liftMoving = false;
  78. desiredLift = SensorValue[liftEncoder];
  79. }
  80.  
  81.  
  82. error = SensorValue[liftEncoder] - desiredLift;
  83.  
  84. integral += error;
  85. if(error == 0 || error>200 || error<-200)
  86. integral = 0;
  87. else if(error>1000 || error<-1000)
  88. error = 1000;
  89. if(derivative==0 && integral==0)
  90. desiredLift = SensorValue[liftEncoder];
  91. if(integral>1500)
  92. integral = 1500;
  93.  
  94. derivative = error - lastError;
  95.  
  96. lastError = error;
  97.  
  98. lastSensorValue = SensorValue[liftEncoder];
  99.  
  100. lift((kP*error + kI*integral + kD*derivative));//*!SensorValue(liftBottom));
  101.  
  102. delay(15);
  103. }
  104. /**while(true){
  105. if(vexRT[Btn5U]){
  106. lift(127);
  107. }
  108. else if(vexRT[Btn5D]){
  109. lift(-127);
  110. }
  111. else{
  112. lift(10);
  113. }
  114. }*/
  115. }
  116.  
  117. task clawDoubleControl(){
  118. while (true){
  119. if(!(SensorValue(clawPot1)<=SensorValue(clawPot2)-50 && SensorValue(clawPot1)>=SensorValue(clawPot2)+50)){
  120. desiredClaw1=desiredClaw2;
  121. }
  122. delay(5);
  123. }
  124. }
  125.  
  126. bool clawMovingOut1 = false;
  127. bool clawInMotion1 = false;
  128. task clawControl1(){
  129. float Kp = 0.15;
  130. float Ki = 0.008;
  131. float Kd = 1.2;
  132. float error;
  133. float integral;
  134. float derivative;
  135. float previous_error;
  136. desiredClaw1 = SensorValue[clawPot1];
  137.  
  138. while(true) {
  139. if(vexRT[Btn6U]) {
  140. if(clawMovingOut1)
  141. desiredClaw1 = SensorValue[clawPot1];
  142. desiredClaw1 -= 70;
  143. clawMovingOut1 = false;
  144. clawInMotion1 = true;
  145. } else if (vexRT[Btn6D]) {
  146. if(!clawMovingOut1)
  147. desiredClaw1 = SensorValue[clawPot1];
  148. desiredClaw1 += 70;
  149. clawMovingOut1 = true;
  150. clawInMotion1 = true;
  151. } else if (clawInMotion1) {
  152. clawInMotion1 = false;
  153. desiredClaw1 = SensorValue[clawPot1]-((!clawMovingOut1)*70);
  154. }
  155.  
  156. error = (desiredClaw1) - (SensorValue[clawPot1]);
  157. integral = integral + error;
  158. if (error == 0) {
  159. integral = 0;
  160. }
  161. if ( abs(error) > 40) {
  162. integral = 0;
  163. }
  164. derivative = error - previous_error;
  165. previous_error = error;
  166. motor[intake1] = Kp*error + Ki*integral + Kd*derivative;
  167. }
  168. }
  169.  
  170. bool clawMovingOut2 = false;
  171. bool clawInMotion2 = false;
  172. task clawControl2(){
  173. float Kp = 0.15;
  174. float Ki = 0.008;
  175. float Kd = 1.2;
  176. float error;
  177. float integral;
  178. float derivative;
  179. float previous_error;
  180. desiredClaw2 = SensorValue[clawPot2];
  181.  
  182. while(true) {
  183. if(vexRT[Btn6U]) {
  184. if(clawMovingOut2)
  185. desiredClaw2 = SensorValue[clawPot2];
  186. desiredClaw2 -= 70;
  187. clawMovingOut2 = false;
  188. clawInMotion2 = true;
  189. } else if (vexRT[Btn6D]) {
  190. if(!clawMovingOut2)
  191. desiredClaw2 = SensorValue[clawPot2];
  192. desiredClaw2 += 70;
  193. clawMovingOut2 = true;
  194. clawInMotion2 = true;
  195. } else if (clawInMotion2) {
  196. clawInMotion2 = false;
  197. desiredClaw2 = SensorValue[clawPot2]-((!clawMovingOut2)*70);
  198. }
  199.  
  200. error = (desiredClaw2) - (SensorValue[clawPot2]);
  201. integral = integral + error;
  202. if (error == 0) {
  203. integral = 0;
  204. }
  205. if ( abs(error) > 40) {
  206. integral = 0;
  207. }
  208. derivative = error - previous_error;
  209. previous_error = error;
  210. motor[intake2] = Kp*error + Ki*integral + Kd*derivative;
  211. }
  212. }
  213.  
  214. void resetEncoders(){
  215. nMotorEncoder[rightDrive]=0;
  216. nMotorEncoder[leftDrive]=0;
  217. nMotorEncoder[liftEncoder]=0;
  218. }
  219.  
  220. void drive(int power){
  221. motor[leftDrive]=motor[rightDrive]=power;
  222. }
  223.  
  224. void autonR(){
  225. /*startTask(clawControl);
  226. startTask(liftControl);
  227. desiredClaw1=1900;
  228. desiredClaw2=1900;
  229. desiredLift=50;
  230. while(nMotorEncoder(rightDrive)<150){
  231. drive(127);
  232. delay(3);
  233. }
  234. drive(0);
  235. desiredClaw1=1900;
  236. desiredClaw2=1900;
  237. desiredLift=0;
  238. while(nMotorEncoder(rightDrive)<180){
  239. drive(127);
  240. delay(3);
  241. }
  242. drive(0);
  243. desiredClaw1=1900;
  244. desiredClaw2=1900;
  245. desiredLift=75;
  246. while(nMotorEncoder(rightDrive)<225){
  247. drive(127);
  248. delay(3);
  249. }
  250. drive(0);
  251. while(nMotorEncoder(leftDrive)>160){
  252. motor[leftDrive]=-127;
  253. delay(3);
  254. }
  255. while(nMotorEncoder(rightDrive)<275){
  256. drive(127);
  257. delay(3);
  258. }
  259. drive(0);
  260. motor[leftDrive]=0;
  261. desiredLift=500;
  262. wait1Msec(250);
  263. desiredClaw=500;
  264. wait1Msec(75);
  265. desiredLift=0;
  266. desiredClaw1=1900;
  267. desiredClaw2=1900;
  268. while(nMotorEncoder(rightDrive)<325){
  269. motor[rightDrive]=127;
  270. delay(3);
  271. }
  272. motor[rightDrive]=0;
  273. while(nMotorEncoder(rightDrive)<450){
  274. drive(127);
  275. delay(3);
  276. }
  277. while(nMotorEncoder(leftDrive)<325){
  278. motor[leftDrive]=127;
  279. delay(3);
  280. }
  281. motor[leftDrive]=0;
  282. while(nMotorEncoder(rightDrive)<550){
  283. drive(127);
  284. delay(3);
  285. }
  286. drive(0);
  287. desiredClaw1=1900;
  288. desiredClaw2=1900;
  289. wait1Msec(150);
  290. desiredLift=50;
  291. wait1Msec(100);
  292. while(nMotorEncoder(leftDrive)<450){
  293. motor[leftDrive]=127;
  294. delay(3);
  295. }
  296. motor[leftDrive]=0;
  297. while(nMotorEncoder(rightDrive)<700){
  298. drive(127);
  299. delay(3);
  300. }
  301. drive(0);
  302. desiredClaw1=1900;
  303. desiredClaw2=1900;
  304. wait1Msec(250);
  305. desiredClaw1=1900;
  306. desiredClaw2=1900;
  307. wait1Msec(75);
  308. desiredLift=0;
  309. desiredClaw1=1900;
  310. desiredClaw2=1900;*/
  311. }
  312.  
  313. void autonL(){
  314. //add this by changing sides
  315. wait1Msec(100);
  316. }
  317.  
  318. void pre_auton() {
  319. resetEncoders();
  320. }
  321.  
  322. task autonomous {
  323. /**
  324. lift(127);
  325. wait1Msec(200);
  326. lift(0);
  327. drive(-127);
  328. wait1Msec(100);
  329. drive(0);
  330. motor[intake1]=127;
  331. motor[intake2]=127;
  332. wait1Msec(500);
  333. motor[intake1]=0;
  334. motor[intake2]=0;
  335. lift(127);
  336. wait1Msec(50);
  337. lift(0);
  338. drive(-127);
  339. wait1Msec(1100);
  340. drive(10);
  341. lift(127);
  342. wait1Msec(700);
  343. motor[intake1]=127;
  344. motor[intake2]=127;
  345. wait1Msec(300);
  346. drive(0);
  347. motor[intake1]=0;
  348. motor[intake2]=0;
  349. */
  350. autonR();
  351. }
  352.  
  353. task usercontrol(){
  354. startTask(clawControl1);
  355. startTask(clawControl2);
  356. startTask(clawDoubleControl);
  357. startTask(liftControl);
  358. startTask(driveControl);
  359. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!