API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Aug 15th, 2018
255
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 13.82 KB | None | 0 0
raw download clone embed print report
  1. Odrive.ino:
  2. #include <SoftwareSerial.h>
  3. #include <ODriveArduino.h>
  4.  
  5.  
  6. //BEGIN ODRIVE DEPENDECIES
  7. // Printing with stream operator
  8. template<class T> inline Print& operator <<(Print &obj,     T arg) { obj.print(arg);    return obj; }
  9. template<>        inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; }
  10. // Serial to the ODrive
  11. SoftwareSerial odrive_serial(10, 11); //RX (ODrive TX), TX (ODrive RX)
  12. int FLIPPER_01 = 9;
  13. int FLIPPER_02 = 8;
  14. // ODrive object
  15. ODriveArduino odrive(odrive_serial);
  16. int thisGear = 1;
  17. int NC = 1;
  18.  
  19. //FLIPPER 01
  20. int FLIPPER_1_STATE = 0;
  21. int LAST_FLIPPER_1_STATE = 0;
  22. int lastDebounceTime_1 = 0;
  23. int debounceDelay_1 = 0;
  24. int BUTTON_FLIPPER_1_STATE = 0;
  25.  
  26. //FLIPPER 02.
  27. int FLIPPER_2_STATE = 0;
  28. int LAST_FLIPPER_2_STATE = 0;
  29. int lastDebounceTime_2 = 0;
  30. int debounceDelay_2 = 0;
  31. int BUTTON_FLIPPER_2_STATE = 0;
  32. int GO_TO_GEAR = 0;
  33. #include "leddisplay.h"
  34. #include "shifts.h"
  35. #include "calibrate.h"
  36.  
  37. //END ODRIVE DEPENDECIES
  38.  
  39.  
  40.  
  41. void setup() {
  42.   pinMode(FLIPPER_01, INPUT_PULLUP);
  43.   pinMode(FLIPPER_02, INPUT_PULLUP);
  44.   setupLeds();
  45.   //BEGIN ODRIVE DEPENDECIES
  46.  
  47.     odrive_serial.begin(115200);  //Odrive Serial port
  48.       Serial.begin(115200);       //arduino - pc serial
  49.       while (!Serial) ; // wait for Arduino Serial Monitor to open
  50.  
  51.       Serial.println("ODriveArduino");
  52.       Serial.println("Setting parameters...");
  53.    for (int axis = 0; axis < 2; ++axis) {
  54.     odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 8500.0f << '\n'; //FeedRate Limit
  55.     odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 1.0f << '\n';       //Current Limit
  56.    }
  57.   Serial.println("Send the character '0' or '1' to calibrate respective motor");
  58.   Serial.println("Send the character '+' or '-' to shift gears");
  59.  
  60.   //calibrateO(); //Calibrate Motor 0
  61.   //calibrate1(); //Calibrate Motor 1
  62.   //END ODRIVE DEPENDECIES
  63.   // put your setup code here, to run once:
  64. ledFullOn();
  65. }
  66.  
  67. void loop() {
  68.   // put your main code here, to run repeatedly:
  69.   testMoves();
  70.   shiftme();
  71.   readFlipperOne();
  72.   readFlipperTwo();
  73. }
  74.  
  75. void readFlipperOne ()
  76. {
  77.   int READ_FLIPPER_1 = digitalRead(FLIPPER_01);
  78.   if(READ_FLIPPER_1 != LAST_FLIPPER_1_STATE)
  79.   {
  80.     lastDebounceTime_1 = millis();
  81.   }
  82.   if ((millis() - lastDebounceTime_1) > debounceDelay_1) {
  83.     if (READ_FLIPPER_1 != BUTTON_FLIPPER_1_STATE)
  84.     {
  85.       BUTTON_FLIPPER_1_STATE = READ_FLIPPER_1;
  86.       if (BUTTON_FLIPPER_1_STATE == NC && GO_TO_GEAR < 7)
  87.       {
  88.         //Serial.println("Button 1 did something");
  89.           GO_TO_GEAR += 1;
  90.           FLIPPER_1_STATE = !FLIPPER_1_STATE;
  91.       }
  92.  
  93.      }
  94.   }
  95.     LAST_FLIPPER_1_STATE = READ_FLIPPER_1;
  96. }
  97.  
  98. void readFlipperTwo ()
  99. {
  100.   int READ_FLIPPER_2 = digitalRead(FLIPPER_02);
  101.   if(READ_FLIPPER_2 != LAST_FLIPPER_2_STATE)
  102.   {
  103.     lastDebounceTime_2 = millis();
  104.   }
  105.   if ((millis() - lastDebounceTime_2) > debounceDelay_2) {
  106.     if (READ_FLIPPER_2 != BUTTON_FLIPPER_2_STATE)
  107.     {
  108.       BUTTON_FLIPPER_2_STATE = READ_FLIPPER_2;
  109.       if (BUTTON_FLIPPER_2_STATE == NC && GO_TO_GEAR > 0)
  110.       {
  111.         //Serial.println("Button 1 did something");
  112.           GO_TO_GEAR -= 1;
  113.           FLIPPER_2_STATE = !FLIPPER_2_STATE;
  114.       }
  115.  
  116.      }
  117.   }
  118.     LAST_FLIPPER_2_STATE = READ_FLIPPER_2;
  119. }
  120.  
  121.  
  122. calibrate.h:
  123.  
  124. void calibrateO()
  125. {
  126. int requested_state;
  127.     requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
  128.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  129.     odrive.run_state(atoi(0), requested_state, true);
  130.  
  131.     requested_state = ODriveArduino::AXIS_STATE_ENCODER_INDEX_SEARCH;
  132.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  133.     odrive.run_state(atoi(0), requested_state, true);
  134.  
  135.     requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL ;
  136.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  137.     odrive.run_state(atoi(0), requested_state, false); // don't wait
  138.  
  139.     //odrive.save_configuration();
  140.     delay(5);
  141.     Serial.println("Saved Config");
  142. }
  143. void calibrate1()
  144. {
  145. int requested_state;
  146.     requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
  147.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  148.     odrive.run_state(atoi(0), requested_state, true);
  149.  
  150.     requested_state = ODriveArduino::AXIS_STATE_ENCODER_INDEX_SEARCH;
  151.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  152.     odrive.run_state(atoi(0), requested_state, true);
  153.  
  154.     requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL ;
  155.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  156.     odrive.run_state(atoi(0), requested_state, false); // don't wait
  157.  
  158.     //odrive.save_configuration();
  159.     delay(5);
  160.     Serial.println("Saved Config");
  161. }
  162. void testMoves()
  163. {
  164.   if (Serial.available())
  165.   {
  166.     char c = Serial.read();
  167.     if (c == '0')
  168.     {
  169.        calibrateO();
  170.     }
  171.     if (c == '+')
  172.     {
  173.       //odrive.SetPosition(0, 1000);
  174.       if(thisGear < 7) {thisGear +=1;  }    
  175.     }
  176.     if (c == '-')
  177.     {
  178.       //odrive.SetPosition(0, 0);
  179.       if(thisGear > 0) {thisGear -=1;  }  
  180.     }
  181.     if (c == '2')
  182.     {
  183.       odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n";
  184.           Serial << odrive.readFloat() << '\t';  
  185.     }
  186.    
  187.   }
  188. }
  189.  
  190.  
  191.  
  192. leddisplay.h:
  193. /*
  194.  * ---|--|--|--|--|---
  195.  * ---1--2--g--3--4---
  196.  *
  197.  * ---|--|--|--|--|---
  198.  * ---5--6--g--7--8
  199.  *
  200.  *     2_
  201.  *    3| |1
  202.  *    4 -
  203.  *    6| |8
  204.        7-
  205.           5'
  206.  
  207.  */
  208.  
  209.  int led1 = 36;
  210.  int led2 = 37;
  211.  int led3 = 38;
  212.  int led4 = 39;
  213.  int led5 = 40;
  214.  int led6 = 43;
  215.  int led7 = 42;
  216.  int led8 = 41;
  217.  
  218. void setupLeds()
  219. {
  220.   pinMode(led1, OUTPUT);
  221.   pinMode(led2, OUTPUT);
  222.   pinMode(led3, OUTPUT);
  223.   pinMode(led4, OUTPUT);
  224.   pinMode(led5, OUTPUT);
  225.   pinMode(led6, OUTPUT);
  226.   pinMode(led7, OUTPUT);
  227.   pinMode(led8, OUTPUT);
  228.  
  229. }
  230. void ledFullOn()
  231. {
  232.   digitalWrite(led1, LOW);
  233.   digitalWrite(led2, LOW);
  234.   digitalWrite(led3, LOW);
  235.   digitalWrite(led4, LOW);
  236.   digitalWrite(led5, LOW);
  237.   digitalWrite(led6, LOW);
  238.   digitalWrite(led7, LOW);
  239.   digitalWrite(led8, LOW);
  240.   delay(100);
  241.   digitalWrite(led1, HIGH);
  242.   digitalWrite(led2, HIGH);
  243.   digitalWrite(led3, HIGH);
  244.   digitalWrite(led4, HIGH);
  245.   digitalWrite(led5, HIGH);
  246.   digitalWrite(led6, HIGH);
  247.   digitalWrite(led7, HIGH);
  248.   digitalWrite(led8, HIGH);
  249.   delay(1000);
  250. }
  251. void ledOne()
  252. {
  253.   digitalWrite(led1, LOW);
  254.   digitalWrite(led2, LOW);
  255.   digitalWrite(led3, LOW);
  256.   digitalWrite(led4, LOW);
  257.   digitalWrite(led5, LOW);
  258.   digitalWrite(led6, LOW);
  259.   digitalWrite(led7, LOW);
  260.   digitalWrite(led8, LOW);
  261.   delay(10);
  262.   digitalWrite(led1, HIGH);
  263.   digitalWrite(led2, LOW);
  264.   digitalWrite(led3, LOW);
  265.   digitalWrite(led4, LOW);
  266.   digitalWrite(led5, LOW);
  267.   digitalWrite(led6, LOW);
  268.   digitalWrite(led7, LOW);
  269.   digitalWrite(led8, HIGH);
  270. }
  271. void ledTwo()
  272. {
  273.   digitalWrite(led1, LOW);
  274.   digitalWrite(led2, LOW);
  275.   digitalWrite(led3, LOW);
  276.   digitalWrite(led4, LOW);
  277.   digitalWrite(led5, LOW);
  278.   digitalWrite(led6, LOW);
  279.   digitalWrite(led7, LOW);
  280.   digitalWrite(led8, LOW);
  281.   delay(10);
  282.   digitalWrite(led1, HIGH);
  283.   digitalWrite(led2, HIGH);
  284.   digitalWrite(led3, LOW);
  285.   digitalWrite(led4, HIGH);
  286.   digitalWrite(led5, LOW);
  287.   digitalWrite(led6, HIGH);
  288.   digitalWrite(led7, HIGH);
  289.   digitalWrite(led8, LOW);
  290. }
  291. void ledThree()
  292. {
  293.   digitalWrite(led1, LOW);
  294.   digitalWrite(led2, LOW);
  295.   digitalWrite(led3, LOW);
  296.   digitalWrite(led4, LOW);
  297.   digitalWrite(led5, LOW);
  298.   digitalWrite(led6, LOW);
  299.   digitalWrite(led7, LOW);
  300.   digitalWrite(led8, LOW);
  301.   delay(10);
  302.   digitalWrite(led1, HIGH);
  303.   digitalWrite(led2, HIGH);
  304.   digitalWrite(led3, LOW);
  305.   digitalWrite(led4, HIGH);
  306.   digitalWrite(led5, LOW);
  307.   digitalWrite(led6, LOW);
  308.   digitalWrite(led7, HIGH);
  309.   digitalWrite(led8, HIGH);
  310. }
  311. void ledFour()
  312. {
  313.   digitalWrite(led1, LOW);
  314.   digitalWrite(led2, LOW);
  315.   digitalWrite(led3, LOW);
  316.   digitalWrite(led4, LOW);
  317.   digitalWrite(led5, LOW);
  318.   digitalWrite(led6, LOW);
  319.   digitalWrite(led7, LOW);
  320.   digitalWrite(led8, LOW);
  321.   delay(10);
  322.   digitalWrite(led1, HIGH);
  323.   digitalWrite(led2, LOW);
  324.   digitalWrite(led3, HIGH);
  325.   digitalWrite(led4, HIGH);
  326.   digitalWrite(led5, LOW);
  327.   digitalWrite(led6, LOW);
  328.   digitalWrite(led7, LOW);
  329.   digitalWrite(led8, HIGH);
  330. }
  331. void ledFive()
  332. {
  333.   digitalWrite(led1, LOW);
  334.   digitalWrite(led2, LOW);
  335.   digitalWrite(led3, LOW);
  336.   digitalWrite(led4, LOW);
  337.   digitalWrite(led5, LOW);
  338.   digitalWrite(led6, LOW);
  339.   digitalWrite(led7, LOW);
  340.   digitalWrite(led8, LOW);
  341.   delay(10);
  342.   digitalWrite(led1, LOW);
  343.   digitalWrite(led2, HIGH);
  344.   digitalWrite(led3, HIGH);
  345.   digitalWrite(led4, HIGH);
  346.   digitalWrite(led5, LOW);
  347.   digitalWrite(led6, LOW);
  348.   digitalWrite(led7, HIGH);
  349.   digitalWrite(led8, HIGH);
  350. }
  351. void ledSix()
  352. {
  353.   digitalWrite(led1, LOW);
  354.   digitalWrite(led2, LOW);
  355.   digitalWrite(led3, LOW);
  356.   digitalWrite(led4, LOW);
  357.   digitalWrite(led5, LOW);
  358.   digitalWrite(led6, LOW);
  359.   digitalWrite(led7, LOW);
  360.   digitalWrite(led8, LOW);
  361.   delay(10);
  362.   digitalWrite(led1, LOW);
  363.   digitalWrite(led2, HIGH);
  364.   digitalWrite(led3, HIGH);
  365.   digitalWrite(led4, HIGH);
  366.   digitalWrite(led5, LOW);
  367.   digitalWrite(led6, HIGH);
  368.   digitalWrite(led7, HIGH);
  369.   digitalWrite(led8, HIGH);
  370. }
  371. void ledSeven()
  372. {
  373.   digitalWrite(led1, LOW);
  374.   digitalWrite(led2, LOW);
  375.   digitalWrite(led3, LOW);
  376.   digitalWrite(led4, LOW);
  377.   digitalWrite(led5, LOW);
  378.   digitalWrite(led6, LOW);
  379.   digitalWrite(led7, LOW);
  380.   digitalWrite(led8, LOW);
  381.   delay(10);
  382.   digitalWrite(led1, HIGH);
  383.   digitalWrite(led2, HIGH);
  384.   digitalWrite(led3, LOW);
  385.   digitalWrite(led4, LOW);
  386.   digitalWrite(led5, LOW);
  387.   digitalWrite(led6, LOW);
  388.   digitalWrite(led7, LOW);
  389.   digitalWrite(led8, HIGH);
  390. }
  391. void ledEight()
  392. {
  393.   digitalWrite(led1, LOW);
  394.   digitalWrite(led2, LOW);
  395.   digitalWrite(led3, LOW);
  396.   digitalWrite(led4, LOW);
  397.   digitalWrite(led5, LOW);
  398.   digitalWrite(led6, LOW);
  399.   digitalWrite(led7, LOW);
  400.   digitalWrite(led8, LOW);
  401.   delay(10);
  402.   digitalWrite(led1, HIGH);
  403.   digitalWrite(led2, HIGH);
  404.   digitalWrite(led3, HIGH);
  405.   digitalWrite(led4, HIGH);
  406.   digitalWrite(led5, LOW);
  407.   digitalWrite(led6, HIGH);
  408.   digitalWrite(led7, HIGH);
  409.   digitalWrite(led8, HIGH);
  410. }
  411. void ledNine()
  412. {
  413.   digitalWrite(led1, LOW);
  414.   digitalWrite(led2, LOW);
  415.   digitalWrite(led3, LOW);
  416.   digitalWrite(led4, LOW);
  417.   digitalWrite(led5, LOW);
  418.   digitalWrite(led6, LOW);
  419.   digitalWrite(led7, LOW);
  420.   digitalWrite(led8, LOW);
  421.   delay(10);
  422.   digitalWrite(led1, HIGH);
  423.   digitalWrite(led2, HIGH);
  424.   digitalWrite(led3, HIGH);
  425.   digitalWrite(led4, HIGH);
  426.   digitalWrite(led5, LOW);
  427.   digitalWrite(led6, LOW);
  428.   digitalWrite(led7, HIGH);
  429.   digitalWrite(led8, HIGH);
  430. }
  431. void ledZero()
  432. {
  433.   digitalWrite(led1, LOW);
  434.   digitalWrite(led2, LOW);
  435.   digitalWrite(led3, LOW);
  436.   digitalWrite(led4, LOW);
  437.   digitalWrite(led5, LOW);
  438.   digitalWrite(led6, LOW);
  439.   digitalWrite(led7, LOW);
  440.   digitalWrite(led8, LOW);
  441.   delay(10);
  442.   digitalWrite(led1, HIGH);
  443.   digitalWrite(led2, HIGH);
  444.   digitalWrite(led3, HIGH);
  445.   digitalWrite(led4, LOW);
  446.   digitalWrite(led5, HIGH);
  447.   digitalWrite(led6, HIGH);
  448.   digitalWrite(led7, LOW);
  449.   digitalWrite(led8, HIGH);
  450. }
  451. void ledA()
  452. {
  453.   digitalWrite(led1, LOW);
  454.   digitalWrite(led2, LOW);
  455.   digitalWrite(led3, LOW);
  456.   digitalWrite(led4, LOW);
  457.   digitalWrite(led5, LOW);
  458.   digitalWrite(led6, LOW);
  459.   digitalWrite(led7, LOW);
  460.   digitalWrite(led8, LOW);
  461.   delay(10);
  462.   digitalWrite(led1, HIGH);
  463.   digitalWrite(led2, HIGH);
  464.   digitalWrite(led3, HIGH);
  465.   digitalWrite(led4, HIGH);
  466.   digitalWrite(led5, HIGH);
  467.   digitalWrite(led6, HIGH);
  468.   digitalWrite(led7, LOW);
  469.   digitalWrite(led8, HIGH);
  470. }
  471.  
  472.  
  473.  
  474. shifts.h:
  475. #define bar_01_shiftPos_01 10000.0
  476. #define bar_02_shiftPos_01 10000.0
  477. #define bar_01_shiftPos_02 20000.0
  478. #define bar_02_shiftPos_02 20000.0
  479. #define bar_01_shiftPos_03 30000.0
  480. #define bar_02_shiftPos_03 30000.0
  481. #define bar_01_shiftPos_04 40000.0
  482. #define bar_02_shiftPos_04 40000.0
  483. #define bar_01_shiftPos_05 50000.0
  484. #define bar_02_shiftPos_05 50000.0
  485. #define bar_01_shiftPos_06 60000.0
  486. #define bar_02_shiftPos_06 60000.0
  487. #define bar_01_shiftPos_07 70000.0
  488. #define bar_02_shiftPos_07 70000.0
  489. #define bar_01_shiftPos_08 80000.0
  490. #define bar_02_shiftPos_08 80000.0
  491. int shift = 1;
  492.  
  493. void shiftme()
  494. {
  495.   shift = GO_TO_GEAR;
  496. if (shift == 0) {
  497.  
  498.     //Serial.println("Moving to gear Reverse");
  499.   ledA();
  500.   odrive.SetPosition(0, bar_01_shiftPos_01);
  501.   odrive.SetPosition(1, bar_02_shiftPos_01);
  502.   shift = -1;
  503. }
  504. else if (shift == 1) {
  505.  
  506.   //  Serial.println("Moving to gear Neutral");
  507.   ledZero();
  508.   odrive.SetPosition(0, bar_01_shiftPos_02);
  509.   odrive.SetPosition(1, bar_02_shiftPos_02);
  510.   shift = -1;
  511. }
  512. else if (shift == 2) {
  513.  
  514.   //  Serial.println("Moving to gear 1");
  515.   ledOne();
  516.   odrive.SetPosition(0, bar_01_shiftPos_03);
  517.   odrive.SetPosition(1, bar_02_shiftPos_03);
  518.   shift = -1;
  519. }
  520. else if (shift == 3) {
  521.  
  522.    // Serial.println("Moving to gear 2");
  523.   ledTwo();
  524.   odrive.SetPosition(0, bar_01_shiftPos_04);
  525.   odrive.SetPosition(1, bar_02_shiftPos_04);
  526.   shift = -1;
  527. }
  528. else if (shift == 4) {
  529.  
  530.    // Serial.println("Moving to gear 3");
  531.   ledThree();
  532.   odrive.SetPosition(0, bar_01_shiftPos_05);
  533.   odrive.SetPosition(1, bar_02_shiftPos_05);
  534.   shift = -1;
  535. }
  536. else if (shift == 5) {
  537.  
  538.    // Serial.println("Moving to gear 4");
  539.   ledFour();
  540.   odrive.SetPosition(0, bar_01_shiftPos_06);
  541.   odrive.SetPosition(1, bar_02_shiftPos_06);
  542.   shift = -1;
  543. }
  544. else if (shift == 6) {
  545.  
  546.     //Serial.println("Moving to gear 5");
  547.   ledFive();
  548.   odrive.SetPosition(0, bar_01_shiftPos_07);
  549.   odrive.SetPosition(1, bar_02_shiftPos_07);
  550.   shift = -1;
  551. }
  552. else if (shift == 7) {
  553.  
  554.     //Serial.println("Moving to gear 6");
  555.   ledSix();
  556.   odrive.SetPosition(0, bar_01_shiftPos_08);
  557.   odrive.SetPosition(1, bar_02_shiftPos_08);
  558.   shift = -1;
  559. }
  560. }
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!