API tools faq
paste
ChristosS

Arm Sketch 10.7

ChristosS
Apr 27th, 2018
198
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 15.51 KB | None | 0 0
raw download clone embed print report
  1. #include "VARIABLES.h"
  2. #include <Wire.h>
  3. #include <Adafruit_PWMServoDriver.h>
  4. #include <Arduino.h>
  5. #include <Adafruit_ADS1015.h>
  6. #include "Adafruit_MCP23017.h"
  7. #include "DRV8825.h"
  8. Adafruit_PWMServoDriver ServoDriver = Adafruit_PWMServoDriver();
  9. Adafruit_MCP23017 mcp;
  10. Adafruit_ADS1115 ads;
  11. DRV8825 stepper(MOTOR_STEPS, DIR, STEP, ENABLE, MODE0, MODE1, MODE2);
  12. void drive_claw()
  13. {
  14. if(buttonState[2]==LOW)
  15. {
  16. if(ClawAngle-1>=0)
  17. {
  18. ClawAngle--;
  19. drive_servo(4,ClawAngle);
  20. }
  21. }
  22. if(buttonState[3]==LOW)
  23. {
  24. if(ClawAngle+1<=180)
  25. {
  26. ClawAngle++;
  27. drive_servo(4,ClawAngle);
  28. }
  29. }
  30. //Serial.print("CLAW ANGLE : ");
  31. //Serial.println(ClawAngle);
  32. }
  33. int drive_servo(int servo,int DesiredPos)
  34. {
  35. if(DesiredPos >=0 && DesiredPos <=180)
  36. {
  37. if(servo==0) //BASE SERVO
  38. {
  39. if(DesiredPos!=BasePos)
  40. {
  41. if(DesiredPos>BasePos)
  42. {
  43. if(DesiredPos-BasePos>20)
  44. {
  45. temp=BasePos+10;
  46. while(BasePos<=temp) //While in the first 10 degrees
  47. {
  48. move_servo(0,BasePos);
  49. move_servo(1,BasePos);
  50. BasePos++;
  51. delayMicroseconds(StartServoDelay);
  52. //Serial.print("BasePos ");
  53. //Serial.println(BasePos);
  54. }
  55. temp=DesiredPos-10;
  56. while(BasePos <=temp) //While in the middle of the move till 10 degrees from target angle
  57. {
  58. move_servo(0,BasePos);
  59. move_servo(1,BasePos);
  60. BasePos++;
  61. delayMicroseconds(ServoMoveDelay);
  62. //Serial.print("BasePos ");
  63. //Serial.println(BasePos);
  64. }
  65. temp=0;
  66. while(BasePos <=DesiredPos)
  67. {
  68. move_servo(0,BasePos);
  69. move_servo(1,BasePos);
  70. BasePos++;
  71. delayMicroseconds(StopServoDelay);
  72. //Serial.print("BasePos ");
  73. //Serial.println(BasePos);
  74. }
  75. }
  76. else
  77. {
  78. while(BasePos <=DesiredPos)
  79. {
  80. move_servo(0,BasePos);
  81. move_servo(1,BasePos);
  82. BasePos++;
  83. delayMicroseconds(ServoMoveDelay);
  84. //Serial.print("BasePos ");
  85. //Serial.println(BasePos);
  86. }
  87. }
  88. }
  89. if(DesiredPos<BasePos)
  90. {
  91. if(BasePos-DesiredPos>20)
  92. {
  93. temp=BasePos-10;
  94. while(BasePos>=temp) //While in the first 10 degrees
  95. {
  96. move_servo(0,BasePos);
  97. move_servo(1,BasePos);
  98. BasePos--;
  99. delayMicroseconds(StartServoDelay);
  100. //Serial.print("BasePos ");
  101. //Serial.println(BasePos);
  102. }
  103. temp=DesiredPos+10;
  104. while(BasePos >=temp) //While in the middle of the move till 10 degrees from target angle
  105. {
  106. move_servo(0,BasePos);
  107. move_servo(1,BasePos);
  108. BasePos--;
  109. delayMicroseconds(ServoMoveDelay);
  110. //Serial.print("BasePos ");
  111. ////Serial.println(BasePos);
  112. }
  113. temp=0;
  114. while(BasePos >=DesiredPos)
  115. {
  116. move_servo(0,BasePos);
  117. move_servo(1,BasePos);
  118. BasePos--;
  119. delayMicroseconds(StopServoDelay);
  120. //Serial.print("BasePos ");
  121. //Serial.println(BasePos);
  122. }
  123. }
  124. else
  125. {
  126. while(BasePos >=DesiredPos)
  127. {
  128. move_servo(0,BasePos);
  129. move_servo(1,BasePos);
  130. BasePos--;
  131. delayMicroseconds(ServoMoveDelay);
  132. //Serial.print("BasePos ");
  133. //Serial.println(BasePos);
  134. }
  135. }
  136. }
  137. }
  138. }
  139. if(servo==1) //J1 SERVO
  140. {
  141. if(DesiredPos!=J2Pos)
  142. {
  143. if(DesiredPos>J2Pos)
  144. {
  145. if(DesiredPos-J2Pos>20)
  146. {
  147. temp=J2Pos+10;
  148. while(J2Pos<=temp) //While in the first 10 degrees
  149. {
  150. move_servo(2,J2Pos);
  151. J2Pos++;
  152. delayMicroseconds(StartServoDelay);
  153. //Serial.print("J2Pos ");
  154. //Serial.println(J2Pos);
  155. }
  156. temp=DesiredPos-10;
  157. while(J2Pos <=temp) //While in the middle of the move till 10 degrees from target angle
  158. {
  159. move_servo(2,J2Pos);
  160. J2Pos++;
  161. delayMicroseconds(ServoMoveDelay);
  162. //Serial.print("J2Pos ");
  163. //Serial.println(J2Pos);
  164. }
  165. temp=0;
  166. while(J2Pos <=DesiredPos)
  167. {
  168. move_servo(2,J2Pos);
  169. J2Pos++;
  170. delayMicroseconds(StopServoDelay);
  171. //Serial.print("J2Pos ");
  172. //Serial.println(J2Pos);
  173. }
  174. }
  175. else
  176. {
  177. while(J2Pos <=DesiredPos)
  178. {
  179. move_servo(2,J2Pos);
  180. J2Pos++;
  181. delayMicroseconds(ServoMoveDelay);
  182. //Serial.print("J2Pos ");
  183. //Serial.println(J2Pos);
  184. }
  185. }
  186. }
  187. if(DesiredPos<J2Pos)
  188. {
  189. if(J2Pos-DesiredPos>20)
  190. {
  191. temp=J2Pos-10;
  192. while(J2Pos>=temp) //While in the first 10 degrees
  193. {
  194. move_servo(2,J2Pos);
  195. J2Pos--;
  196. delayMicroseconds(StartServoDelay);
  197. //Serial.print("J2Pos ");
  198. //Serial.println(J2Pos);
  199. }
  200. temp=DesiredPos+10;
  201. while(J2Pos >=temp) //While in the middle of the move till 10 degrees from target angle
  202. {
  203. move_servo(2,J2Pos);
  204. J2Pos--;
  205. delayMicroseconds(ServoMoveDelay);
  206. //Serial.print("J2Pos ");
  207. //Serial.println(J2Pos);
  208. }
  209. temp=0;
  210. while(J2Pos >=DesiredPos)
  211. {
  212. move_servo(2,J2Pos);
  213. J2Pos--;
  214. delayMicroseconds(StopServoDelay);
  215. //Serial.print("J2Pos ");
  216. //Serial.println(J2Pos);
  217. }
  218. }
  219. else
  220. {
  221. while(J2Pos >=DesiredPos)
  222. {
  223. move_servo(2,J2Pos);
  224. J2Pos--;
  225. delayMicroseconds(ServoMoveDelay);
  226. //Serial.print("J2Pos ");
  227. //Serial.println(J2Pos);
  228. }
  229. }
  230. }
  231. }
  232. }
  233. if(servo==2) //J3 SERVO
  234. {
  235.  
  236. if(DesiredPos!=J3Pos)
  237. {
  238. if(DesiredPos>J3Pos)
  239. {
  240. if(DesiredPos-J3Pos>20)
  241. {
  242. temp=J3Pos+10;
  243. while(J3Pos<=temp) //While in the first 10 degrees
  244. {
  245. move_servo(3,J3Pos);
  246. J3Pos++;
  247. delayMicroseconds(StartServoDelay);
  248. //Serial.print("J3Pos ");
  249. //Serial.println(J3Pos);
  250. }
  251. temp=DesiredPos-10;
  252. while(J3Pos <=temp) //While in the middle of the move till 10 degrees from target angle
  253. {
  254. move_servo(3,J3Pos);
  255. J3Pos++;
  256. delayMicroseconds(ServoMoveDelay);
  257. //Serial.print("J3Pos ");
  258. //Serial.println(J3Pos);
  259. }
  260. temp=0;
  261. while(J3Pos <=DesiredPos)
  262. {
  263. move_servo(3,J3Pos);
  264. J3Pos++;
  265. delayMicroseconds(StopServoDelay);
  266. //Serial.print("J3Pos ");
  267. //Serial.println(J3Pos);
  268. }
  269. }
  270. else
  271. {
  272. while(J3Pos <=DesiredPos)
  273. {
  274. move_servo(3,J3Pos);
  275. J3Pos++;
  276. delayMicroseconds(ServoMoveDelay);
  277. //Serial.print("J3Pos ");
  278. //Serial.println(J3Pos);
  279. }
  280. }
  281. }
  282. if(DesiredPos<J3Pos)
  283. {
  284. if(J3Pos-DesiredPos>20)
  285. {
  286. temp=J3Pos-10;
  287. while(J3Pos>=temp) //While in the first 10 degrees
  288. {
  289. move_servo(3,J3Pos);
  290. J3Pos--;
  291. delayMicroseconds(StartServoDelay);
  292. //Serial.print("J3Pos ");
  293. //Serial.println(J3Pos);
  294. }
  295. temp=DesiredPos+10;
  296. while(J3Pos >=temp) //While in the middle of the move till 10 degrees from target angle
  297. {
  298. move_servo(3,J3Pos);
  299. J3Pos--;
  300. delayMicroseconds(ServoMoveDelay);
  301. //Serial.print("J3Pos ");
  302. //Serial.println(J3Pos);
  303. }
  304. temp=0;
  305. while(J3Pos >=DesiredPos)
  306. {
  307. move_servo(3,J3Pos);
  308. J3Pos--;
  309. delayMicroseconds(StopServoDelay);
  310. //Serial.print("J3Pos ");
  311. //Serial.println(J3Pos);
  312. }
  313. }
  314. else
  315. {
  316. while(J3Pos >=DesiredPos)
  317. {
  318. move_servo(3,J3Pos);
  319. J3Pos--;
  320. delayMicroseconds(ServoMoveDelay);
  321. //Serial.print("J3Pos ");
  322. //Serial.println(J3Pos);
  323. }
  324. }
  325. }
  326. }
  327. }
  328. if(servo==3)
  329. {
  330.  
  331. if(DesiredPos!=WristPos)
  332. {
  333. if(DesiredPos>WristPos)
  334. {
  335. if(DesiredPos-WristPos>20)
  336. {
  337. temp=WristPos+10;
  338. while(WristPos<=temp) //While in the first 10 degrees
  339. {
  340. move_servo(4,WristPos);
  341. WristPos++;
  342. delayMicroseconds(StartServoDelay);
  343. //Serial.print("WristPos ");
  344. //Serial.println(WristPos);
  345. }
  346. temp=DesiredPos-10;
  347. while(WristPos <=temp) //While in the middle of the move till 10 degrees from target angle
  348. {
  349. move_servo(4,WristPos);
  350. WristPos++;
  351. delayMicroseconds(ServoMoveDelay);
  352. //Serial.print("WristPos ");
  353. //Serial.println(WristPos);
  354. }
  355. temp=0;
  356. while(WristPos <=DesiredPos)
  357. {
  358. move_servo(4,WristPos);
  359. WristPos++;
  360. delayMicroseconds(StopServoDelay);
  361. //Serial.print("WristPos ");
  362. //Serial.println(WristPos);
  363. }
  364. }
  365. else
  366. {
  367. while(WristPos <=DesiredPos)
  368. {
  369. move_servo(4,WristPos);
  370. WristPos++;
  371. delayMicroseconds(ServoMoveDelay);
  372. //Serial.print("WristPos ");
  373. //Serial.println(WristPos);
  374. }
  375. }
  376. }
  377. if(DesiredPos<WristPos)
  378. {
  379. if(WristPos-DesiredPos>20)
  380. {
  381. temp=WristPos-10;
  382. while(WristPos>=temp) //While in the first 10 degrees
  383. {
  384. move_servo(4,WristPos);
  385. WristPos--;
  386. delayMicroseconds(StartServoDelay);
  387. //Serial.print("WristPos ");
  388. //Serial.println(WristPos);
  389. }
  390. temp=DesiredPos+10;
  391. while(WristPos >=temp) //While in the middle of the move till 10 degrees from target angle
  392. {
  393. move_servo(4,WristPos);
  394. WristPos--;
  395. delayMicroseconds(ServoMoveDelay);
  396. //Serial.print("WristPos ");
  397. //Serial.println(WristPos);
  398. }
  399. temp=0;
  400. while(WristPos >=DesiredPos)
  401. {
  402. move_servo(4,WristPos);
  403. WristPos--;
  404. delayMicroseconds(StopServoDelay);
  405. //Serial.print("WristPos ");
  406. //Serial.println(WristPos);
  407. }
  408. }
  409. else
  410. {
  411. while(WristPos >=DesiredPos)
  412. {
  413. move_servo(4,WristPos);
  414. WristPos--;
  415. delayMicroseconds(ServoMoveDelay);
  416. //Serial.print("WristPos ");
  417. //Serial.println(WristPos);
  418. }
  419. }
  420. }
  421. }
  422. }
  423. }
  424. }
  425. int move_servo(int servonum , int angle)
  426. {
  427. pulselen=map(angle,0,180,SERVOMIN,SERVOMAX);
  428. ServoDriver.setPWM(servonum, 0, pulselen);
  429. }
  430. void drive_stepper()
  431. {
  432. stepper.setMicrostep(microsteps);
  433. if(buttonState[0]==LOW && buttonState[4]==HIGH) //While Switch to move is pressed and limit switch is not
  434. {
  435. if(StepperPos-1 >=0)
  436. {
  437. StepperPos--;
  438. stepper.move(microsteps*MOTOR_STEPS/defaultStep);
  439. }
  440. }
  441. if(buttonState[1]==LOW&& buttonState[4]==HIGH)
  442. {
  443. if(StepperPos +1 <=360)
  444. {
  445. StepperPos++;
  446. stepper.move(-microsteps*MOTOR_STEPS/defaultStep);
  447. }
  448. }
  449. Serial.println(StepperPos);
  450. }
  451.  
  452. void home_stepper()
  453. {
  454. stepper.setMicrostep(microsteps);
  455. while(buttonState[4]==HIGH)
  456. {
  457. stepper.move(microsteps*MOTOR_STEPS/360);
  458. read_controls();
  459. Serial.println("HOMING");
  460. //delay(1);
  461. }
  462. StepperPos=180;
  463. stepper.move(-microsteps*MOTOR_STEPS/2);
  464. }
  465. void read_controls()
  466. {
  467. adc0=ads.readADC_SingleEnded(0)/64;
  468. adc1=ads.readADC_SingleEnded(1)/64;
  469. adc2=ads.readADC_SingleEnded(2)/64;
  470. adc3=ads.readADC_SingleEnded(3)/64;
  471. X1Angle=map(adc0,0,356,0,180)-2;
  472. Y1Angle=map(adc1,0,384,0,180)-1;
  473. X2Angle=map(adc2,0,359,0,180)-1;
  474. Y2Angle=map(adc3,0,364,0,180)-1;
  475.  
  476. //Serial.print(X1Angle);
  477. //Serial.print(" , ");
  478. //Serial.print(Y1Angle);
  479. //Serial.print(" , ");
  480. //Serial.print(X2Angle);
  481. //Serial.print(" , ");
  482. //Serial.println(Y2Angle);
  483.  
  484.  
  485. // Reading MCP
  486. for(int i=0;i<=5;i++)
  487. {
  488. buttonState[i]=mcp.digitalRead(i);
  489. }
  490. }
  491.  
  492.  
  493. void setup()
  494. {
  495. Serial.begin(9600);
  496. ads.begin();
  497. mcp.begin();
  498. mcp.pullUp(0,HIGH);
  499. mcp.pullUp(1,HIGH);
  500. mcp.pullUp(2,HIGH);
  501. mcp.pullUp(3,HIGH);
  502. mcp.pullUp(4,HIGH);
  503. mcp.pullUp(5,HIGH);
  504. mcp.pinMode(0, INPUT); //J1Button
  505. mcp.pinMode(1, INPUT); //J2Button
  506. mcp.pinMode(2, INPUT); //Button1
  507. mcp.pinMode(3, INPUT); //Button2
  508. mcp.pinMode(4, INPUT); // Limit Switch
  509. mcp.pinMode(5, INPUT); //Change defaultStep variable
  510. ServoDriver.begin();
  511. ServoDriver.setPWMFreq(60);
  512. stepper.begin(RPM);
  513. stepper.enable();
  514. ServoMoveDelay=300;
  515. for(int i=0;i<=4;i++)
  516. {
  517. drive_servo(i,90);
  518. }
  519. //DEBOUNCE CODE
  520. //for(int i=0;i<=4;i++)
  521. //{
  522. //lastButtonState[i]=HIGH;
  523. //}
  524. ServoMoveDelay=0;
  525. read_controls();
  526. home_stepper();
  527. }
  528. void loop()
  529. {
  530. read_controls();
  531. drive_servo(0,X1Angle);
  532. drive_servo(1,Y1Angle);
  533. drive_servo(2,X2Angle);
  534. drive_servo(3,Y2Angle);
  535. drive_claw();
  536. drive_stepper();
  537.  
  538. }
  539.  
  540.  
  541. Variables.h
  542. #ifndef VARIABLES_H
  543. #define VARIABLES_H
  544. int adc0, adc1, adc2, adc3; // ADC Readings
  545. int X1Angle,Y1Angle,X2Angle,Y2Angle; //Joystick Angle Variables
  546. int SERVOMIN =155; // Servo Pulse Widths
  547. int SERVOMAX =538;
  548. int RPM=50; //Stepper Speed
  549. int MOTOR_STEPS=200;
  550. int defaultStep=32; //MOTOR_STEPS/defaultStep=200/360=1 degrees/Step MOTOR_STEPS/200=1.8 degrees/Step
  551. double pulselength; // Servo Variable
  552. int StepperPos; // Stepper
  553. int BasePos; //Base
  554. int J2Pos; // J2
  555. int J3Pos; //J3
  556. int WristPos; //Wrist
  557. int ClawPos; //Claw
  558. int ClawAngle; //The angle that gets changed using the joystick buttons
  559. int ServoDelay=500; // Servo Delay time In us
  560. int HOME=90; // HOMING ANGlE for the servos
  561. int StepperHome=180; // Stepper Homing Angle
  562. int temp; //Servo Temp Position Variable
  563. int ServoMoveDelay=0;
  564. int StartServoDelay=200;
  565. int StopServoDelay=300;
  566. int pulselen;
  567. int buttonState[6];
  568.  
  569. int microsteps=16; //Stepper Microstepping Value
  570.  
  571. #define ENABLE 7
  572. #define STEP 3
  573. #define DIR 2
  574. #define MODE0 6
  575. #define MODE1 5
  576. #define MODE2 4
  577. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!