API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 20th, 2020
145
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 16.23 KB | None | 0 0
raw download clone embed print report
  1. void sCurveForward(float distance, double velocity, int timeout) {
  2. //vel means velocity
  3. //acell means accelleration
  4. //func means function
  5.  
  6. //drive variable and calculations
  7. double wheelDiameterIN = 3.25;
  8. double travelTargetCM = distance; //distance set to travel
  9. double circumfrence = wheelDiameterIN * 3.141592 * 0.6;
  10. double degreesToRotate = (360 * travelTargetCM) / circumfrence;
  11.  
  12. //acceleration variables
  13. int base = 2; //base int for accel function
  14. int counter = 0; //counter for calculations
  15. double calcVel = velocity; //for deaccel calc
  16. double subtractor = 0; //for deaccel
  17. double subtractor2 = 0; //for deaccel calc
  18.  
  19. //all motor encoder variables
  20. int encoderR = RightMotor.rotation(deg); //encoder value
  21. double motorVelR = 0; //vel based of function
  22. int encoderRb = LeftMotor.rotation(deg); //encoder value
  23. double motorVelRb = 0; //vel based of function
  24. int encoderL = RightMotorBack.rotation(deg); //encoder value
  25. double motorVelL = 0; //vel based of function
  26. int encoderLb = LeftmotorBack.rotation(deg); //encoder value
  27. double motorVelLb = 0; //vel based of function
  28.  
  29. // RIGHT FRONT////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  30.  
  31. //beginning vel curve
  32. while(motorVelR < velocity) {
  33. //accel func (2^(.5 * x) - 3)
  34. motorVelR = Math.pow(base, (.5 * encoderR) - 3);
  35. }
  36. //rounds motor vel
  37. motorVelR = Math.round(motorVelR);
  38.  
  39. //velocity correction
  40. //if motor vel less than target vel, add vel until equal target vel
  41. if(motorVelR < velocity) {
  42. while(motorVelR < velocity) {
  43. motorVelR = motorVelR + 1;
  44. }
  45. }
  46. //if motor vel more than target vel, subtract vel until equal target vel
  47. if(motorVelR > velocity) {
  48. while(motorVelR > velocity) {
  49. motorVelR = motorVelR - 1;
  50. }
  51. }
  52.  
  53. //decreasing curve time calc
  54. while(calcVel > 1) {
  55. //accel func (2^(.5 * x) - 3)
  56. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  57. //subtracts accel func from max vel to create a decreasing accel
  58. calcVel = calcVel - subtractor2;
  59. //counts time to reach 0 vel to know when to deaccel
  60. counter = counter + 1;
  61. }
  62.  
  63. //decreasing curve
  64. //calc if bot needs to deaccel then runs code
  65. while((distance - counter) > encoderR) {
  66. //if motorVel more than 1, deaccel
  67. while(motorVelR > 1) {
  68. //accel func (2^(.5 * x) - 3)
  69. subtractor = Math.pow(base, (.5 * encoderR) - 3);
  70. //subtracts accel func from max vel to create a decreasing accel
  71. motorVelR = motorVelR - subtractor;
  72. }
  73. }
  74. //rounds motorVel
  75. motorVelR = Math.round(motorVelR);
  76. //subtracts motorVel by itself so it is 0 than a decimal
  77. motorVelR = motorVelR - motorVel;
  78.  
  79. // RIGHT BACK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  80.  
  81. //beginning vel curve
  82. while(motorVelRb < velocity) {
  83. //accel func (2^(.5 * x) - 3)
  84. motorVelRb = Math.pow(base, (.5 * encoderRb) - 3);
  85. }
  86. //rounds motor vel
  87. motorVelRb = Math.round(motorVelRb);
  88.  
  89. //velocity correction
  90. //if motor vel less than target vel, add vel until equal target vel
  91. if(motorVelRb < velocity) {
  92. while(motorVelRb < velocity) {
  93. motorVelRb = motorVelRb + 1;
  94. }
  95. }
  96. //if motor vel more than target vel, subtract vel until equal target vel
  97. if(motorVelRb > velocity) {
  98. while(motorVelRb > velocity) {
  99. motorVelRb = motorVelRb - 1;
  100. }
  101. }
  102.  
  103. //decreasing curve time calc
  104. while(calcVel > 1) {
  105. //accel func (2^(.5 * x) - 3)
  106. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  107. //subtracts accel func from max vel to create a decreasing accel
  108. calcVel = calcVel - subtractor2;
  109. //counts time to reach 0 vel to know when to deaccel
  110. counter = counter + 1;
  111. }
  112.  
  113. //decreasing curve
  114. //calc if bot needs to deaccel then runs code
  115. while((distance - counter) > encoderRb) {
  116. //if motorVel more than 1, deaccel
  117. while(motorVelRb > 1) {
  118. //accel func (2^(.5 * x) - 3)
  119. subtractor = Math.pow(base, (.5 * encoderRb) - 3);
  120. //subtracts accel func from max vel to create a decreasing accel
  121. motorVelRb = motorVelRb - subtractor;
  122. }
  123. }
  124. //rounds motorVel
  125. motorVelRb = Math.round(motorVelRb);
  126. //subtracts motorVel by itself so it is 0 than a decimal
  127. motorVelRb = motorVelRb - motorVelRb;
  128.  
  129. // Left Front////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  130.  
  131. //beginning vel curve
  132. while(motorVelL < velocity) {
  133. //accel func (2^(.5 * x) - 3)
  134. motorVelL = Math.pow(base, (.5 * encoderL) - 3);
  135. }
  136. //rounds motor vel
  137. motorVelL = Math.round(motorVelL);
  138.  
  139. //velocity correction
  140. //if motor vel less than target vel, add vel until equal target vel
  141. if(motorVelL < velocity) {
  142. while(motorVelL < velocity) {
  143. motorVelL = motorVelL + 1;
  144. }
  145. }
  146. //if motor vel more than target vel, subtract vel until equal target vel
  147. if(motorVelL > velocity) {
  148. while(motorVelL > velocity) {
  149. motorVelL = motorVelL - 1;
  150. }
  151. }
  152.  
  153. //decreasing curve time calc
  154. while(calcVel > 1) {
  155. //accel func (2^(.5 * x) - 3)
  156. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  157. //subtracts accel func from max vel to create a decreasing accel
  158. calcVel = calcVel - subtractor2;
  159. //counts time to reach 0 vel to know when to deaccel
  160. counter = counter + 1;
  161. }
  162.  
  163. //decreasing curve
  164. //calc if bot needs to deaccel then runs code
  165. while((distance - counter) > encoderL) {
  166. //if motorVel more than 1, deaccel
  167. while(motorVelL > 1) {
  168. //accel func (2^(.5 * x) - 3)
  169. subtractor = Math.pow(base, (.5 * encoderL) - 3);
  170. //subtracts accel func from max vel to create a decreasing accel
  171. motorVelL = motorVelL - subtractor;
  172. }
  173. }
  174. //rounds motorVel
  175. motorVelL = Math.round(motorVelL);
  176. //subtracts motorVel by itself so it is 0 than a decimal
  177. motorVelL = motorVelL - motorVelL;
  178.  
  179. // Left Back////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  180.  
  181. //beginning vel curve
  182. while(motorVelLb < velocity) {
  183. //accel func (2^(.5 * x) - 3)
  184. motorVelLb = Math.pow(base, (.5 * encoderLb) - 3);
  185. }
  186. //rounds motor vel
  187. motorVelLb = Math.round(motorVelLb);
  188.  
  189. //velocity correction
  190. //if motor vel less than target vel, add vel until equal target vel
  191. if(motorVelLb < velocity) {
  192. while(motorVelLb < velocity) {
  193. motorVelLb = motorVelLb + 1;
  194. }
  195. }
  196. //if motor vel more than target vel, subtract vel until equal target vel
  197. if(motorVelLb > velocity) {
  198. while(motorVelLb > velocity) {
  199. motorVelLb = motorVelLb - 1;
  200. }
  201. }
  202.  
  203. //decreasing curve time calc
  204. while(calcVel > 1) {
  205. //accel func (2^(.5 * x) - 3)
  206. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  207. //subtracts accel func from max vel to create a decreasing accel
  208. calcVel = calcVel - subtractor2;
  209. //counts time to reach 0 vel to know when to deaccel
  210. counter = counter + 1;
  211. }
  212.  
  213. //decreasing curve
  214. //calc if bot needs to deaccel then runs code
  215. while((distance - counter) > encoderLb) {
  216. //if motorVel more than 1, deaccel
  217. while(motorVelLb > 1) {
  218. //accel func (2^(.5 * x) - 3)
  219. subtractor = Math.pow(base, (.5 * encoderLb) - 3);
  220. //subtracts accel func from max vel to create a decreasing accel
  221. motorVelLb = motorVelLb - subtractor;
  222. }
  223. }
  224. //rounds motorVel
  225. motorVelLb = Math.round(motorVelLb);
  226. //subtracts motorVel by itself so it is 0 than a decimal
  227. motorVelLb = motorVelLb - motorVelLb;
  228.  
  229. LeftMotor.setVelocity(motorVelL, vex::velocityUnits::pct);
  230. LeftMotor.setReversed(true);
  231. RightMotor.setVelocity(motorVelR, vex::velocityUnits::pct);
  232. RightMotor.setReversed(false);
  233. LeftmotorBack.setVelocity(motorVelLb, vex::velocityUnits::pct);
  234. LeftmotorBack.setReversed(true);
  235. RightMotorBack.setVelocity(motorVelRb, vex::velocityUnits::pct);
  236. RightMotorBack.setReversed(false);
  237.  
  238. LeftMotor.setTimeout(timeout, msec);
  239. RightMotor.setTimeout(timeout, msec);
  240. LeftmotorBack.setTimeout(timeout, msec);
  241. RightMotorBack.setTimeout(timeout, msec);
  242. LeftMotor.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  243. RightMotor.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  244. LeftmotorBack.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  245. RightMotorBack.rotateFor(degreesToRotate, vex::rotationUnits::deg, true);
  246. }
  247.  
  248. void cCurveLeft(float distance, double velocity, double curve int timeout) {
  249. //vel means velocity
  250. //acell means accelleration
  251. //func means function
  252.  
  253. //drive variable and calculations
  254. double wheelDiameterIN = 3.25;
  255. double travelTargetCM = distance; //distance set to travel
  256. double circumfrence = wheelDiameterIN * 3.141592 * 0.6;
  257. double degreesToRotate = (360 * travelTargetCM) / circumfrence;
  258.  
  259. //acceleration variables
  260. int base = 2; //base int for accel function
  261. int counter = 0; //counter for calculations
  262. double calcVel = velocity; //for deaccel calc
  263. double subtractor = 0; //for deaccel
  264. //double cFinal = (velocity / curve);
  265.  
  266. //all motor encoder variables
  267. int encoderR = RightMotor.rotation(deg); //encoder value
  268. double motorVelR = 0; //vel based of function
  269. int encoderRb = LeftMotor.rotation(deg); //encoder value
  270. double motorVelRb = 0; //vel based of function
  271. int encoderL = RightMotorBack.rotation(deg); //encoder value
  272. double motorVelL = 0; //vel based of function
  273. int encoderLb = LeftmotorBack.rotation(deg); //encoder value
  274. double motorVelLb = 0; //vel based of function
  275.  
  276. // Left Front////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  277.  
  278. //beginning vel curve
  279. while(motorVelL < velocity) {
  280. //accel func (2^(.5 * x) - 3)
  281. motorVelL = Math.pow(base, (.5 * encoderL) - 3);
  282. }
  283. //rounds motor vel
  284. motorVelL = Math.round(motorVelL);
  285.  
  286. //velocity correction
  287. //if motor vel less than target vel, add vel until equal target vel
  288. if(motorVelL < velocity) {
  289. while(motorVelL < velocity) {
  290. motorVelL = motorVelL + 1;
  291. }
  292. }
  293. //if motor vel more than target vel, subtract vel until equal target vel
  294. if(motorVelL > velocity) {
  295. while(motorVelL > velocity) {
  296. motorVelL = motorVelL - 1;
  297. }
  298. }
  299.  
  300. //c curve
  301. //if motorVel more than velocity / curve, deaccel
  302. while(motorVelL > curve) {
  303. //accel func (2^(.5 * x) - 3)
  304. subtractor = Math.pow(base, (.5 * encoderL) - 3);
  305. //subtracts accel func from max vel to create a decreasing accel
  306. motorVelL = motorVelL - subtractor;
  307. }
  308. //rounds motorVel
  309. motorVelL = Math.round(motorVelL);
  310.  
  311. // Left Back////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  312.  
  313. //beginning vel curve
  314. while(motorVelLb < velocity) {
  315. //accel func (2^(.5 * x) - 3)
  316. motorVelLb = Math.pow(base, (.5 * encoderLb) - 3);
  317. }
  318. //rounds motor vel
  319. motorVelLb = Math.round(motorVelLb);
  320.  
  321. //velocity correction
  322. //if motor vel less than target vel, add vel until equal target vel
  323. if(motorVelLb < velocity) {
  324. while(motorVelLb < velocity) {
  325. motorVelLb = motorVelLb + 1;
  326. }
  327. }
  328. //if motor vel more than target vel, subtract vel until equal target vel
  329. if(motorVelLb > velocity) {
  330. while(motorVelLb > velocity) {
  331. motorVelLb = motorVelLb - 1;
  332. }
  333. }
  334.  
  335. //c curve
  336. //if motorVel more than velocity / curve, deaccel
  337. while(motorVelLb > curve) {
  338. //accel func (2^(.5 * x) - 3)
  339. subtractor = Math.pow(base, (.5 * encoderLb) - 3);
  340. //subtracts accel func from max vel to create a decreasing accel
  341. motorVelLb = motorVelLb - subtractor;
  342. }
  343. //rounds motorVel
  344. motorVelLb = Math.round(motorVelLb);
  345.  
  346. // RIGHT FRONT////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  347.  
  348. //beginning vel curve
  349. while(motorVelR < velocity) {
  350. //accel func (2^(.5 * x) - 3)
  351. motorVelR = Math.pow(base, (.5 * encoderR) - 3);
  352. }
  353. //rounds motor vel
  354. motorVelR = Math.round(motorVelR);
  355.  
  356. //velocity correction
  357. //if motor vel less than target vel, add vel until equal target vel
  358. if(motorVelR < velocity) {
  359. while(motorVelR < velocity) {
  360. motorVelR = motorVelR + 1;
  361. }
  362. }
  363. //if motor vel more than target vel, subtract vel until equal target vel
  364. if(motorVelR > velocity) {
  365. while(motorVelR > velocity) {
  366. motorVelR = motorVelR - 1;
  367. }
  368. }
  369.  
  370. //decreasing curve time calc
  371. while(calcVel > 1) {
  372. //accel func (2^(.5 * x) - 3)
  373. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  374. //subtracts accel func from max vel to create a decreasing accel
  375. calcVel = calcVel - subtractor2;
  376. //counts time to reach 0 vel to know when to deaccel
  377. counter = counter + 1;
  378. }
  379.  
  380. //decreasing curve
  381. //calc if bot needs to deaccel then runs code
  382. while((distance - counter) > encoderR) {
  383. //if motorVel more than 1, deaccel
  384. while(motorVelR > 1) {
  385. //accel func (2^(.5 * x) - 3)
  386. subtractor = Math.pow(base, (.5 * encoderR) - 3);
  387. //subtracts accel func from max vel to create a decreasing accel
  388. motorVelR = motorVelR - subtractor;
  389. }
  390. }
  391. //rounds motorVel
  392. motorVelR = Math.round(motorVelR);
  393. //subtracts motorVel by itself so it is 0 than a decimal
  394. motorVelR = motorVelR - motorVel;
  395.  
  396. // RIGHT BACK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  397.  
  398. //beginning vel curve
  399. while(motorVelRb < velocity) {
  400. //accel func (2^(.5 * x) - 3)
  401. motorVelRb = Math.pow(base, (.5 * encoderRb) - 3);
  402. }
  403. //rounds motor vel
  404. motorVelRb = Math.round(motorVelRb);
  405.  
  406. //velocity correction
  407. //if motor vel less than target vel, add vel until equal target vel
  408. if(motorVelRb < velocity) {
  409. while(motorVelRb < velocity) {
  410. motorVelRb = motorVelRb + 1;
  411. }
  412. }
  413. //if motor vel more than target vel, subtract vel until equal target vel
  414. if(motorVelRb > velocity) {
  415. while(motorVelRb > velocity) {
  416. motorVelRb = motorVelRb - 1;
  417. }
  418. }
  419.  
  420. //decreasing curve time calc
  421. while(calcVel > 1) {
  422. //accel func (2^(.5 * x) - 3)
  423. subtractor2 = Math.pow(base, (.5 * encoder) - 3);
  424. //subtracts accel func from max vel to create a decreasing accel
  425. calcVel = calcVel - subtractor2;
  426. //counts time to reach 0 vel to know when to deaccel
  427. counter = counter + 1;
  428. }
  429.  
  430. //decreasing curve
  431. //calc if bot needs to deaccel then runs code
  432. while((distance - counter) > encoderRb) {
  433. //if motorVel more than 1, deaccel
  434. while(motorVelRb > 1) {
  435. //accel func (2^(.5 * x) - 3)
  436. subtractor = Math.pow(base, (.5 * encoderRb) - 3);
  437. //subtracts accel func from max vel to create a decreasing accel
  438. motorVelRb = motorVelRb - subtractor;
  439. }
  440. }
  441. //rounds motorVel
  442. motorVelRb = Math.round(motorVelRb);
  443. //subtracts motorVel by itself so it is 0 than a decimal
  444. motorVelRb = motorVelRb - motorVelRb;
  445.  
  446. LeftMotor.setVelocity(motorVelL, vex::velocityUnits::pct);
  447. LeftMotor.setReversed(true);
  448. RightMotor.setVelocity(motorVelR, vex::velocityUnits::pct);
  449. RightMotor.setReversed(false);
  450. LeftmotorBack.setVelocity(motorVelLb, vex::velocityUnits::pct);
  451. LeftmotorBack.setReversed(true);
  452. RightMotorBack.setVelocity(motorVelRb, vex::velocityUnits::pct);
  453. RightMotorBack.setReversed(false);
  454.  
  455. LeftMotor.setTimeout(timeout, msec);
  456. RightMotor.setTimeout(timeout, msec);
  457. LeftmotorBack.setTimeout(timeout, msec);
  458. RightMotorBack.setTimeout(timeout, msec);
  459. LeftMotor.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  460. RightMotor.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  461. LeftmotorBack.rotateFor(degreesToRotate, vex::rotationUnits::deg, false);
  462. RightMotorBack.rotateFor(degreesToRotate, vex::rotationUnits::deg, true);
  463. }
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!