API tools faq
paste
Advertisement
Guest User

Updated code

a guest
Sep 19th, 2018
300
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 8.59 KB | None | 0 0
raw download clone embed print report
  1. ||Main.ino
  2.  
  3. #include <SoftwareSerial.h>
  4. #include <ODriveArduino.h>
  5.  
  6.  
  7. //BEGIN ODRIVE DEPENDECIES
  8. // Printing with stream operator
  9. template<class T> inline Print& operator <<(Print &obj,     T arg) { obj.print(arg);    return obj; }
  10. template<>        inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; }
  11. // Serial to the ODrive
  12. SoftwareSerial odrive_serial(10, 11); //RX (ODrive TX), TX (ODrive RX)
  13. int FLIPPER_01 = 9;
  14. int FLIPPER_02 = 8;
  15. // ODrive object
  16. ODriveArduino odrive(odrive_serial);
  17. int thisGear = 1;
  18. int NC = 0;
  19. int IsShifted = true;
  20. //FLIPPER 01
  21. int FLIPPER_1_STATE = 0;
  22. int LAST_FLIPPER_1_STATE = 0;
  23. int lastDebounceTime_1 = 0;
  24. int debounceDelay_1 = 0;
  25. int BUTTON_FLIPPER_1_STATE = 0;
  26.  
  27. //FLIPPER 02.
  28. int FLIPPER_2_STATE = 0;
  29. int LAST_FLIPPER_2_STATE = 0;
  30. int lastDebounceTime_2 = 0;
  31. int debounceDelay_2 = 0;
  32. int BUTTON_FLIPPER_2_STATE = 0;
  33. int GO_TO_GEAR = 1;
  34.  
  35. bool reverse = false;
  36. #include "stepper.h"
  37. #include "leddisplay.h"
  38. #include "shifts.h"
  39. #include "calibrate.h"
  40.  
  41.  
  42. int revButton = 44;
  43.  
  44. //END ODRIVE DEPENDECIES
  45.  
  46.  
  47.  
  48. void setup() {
  49.   pinMode(FLIPPER_01, INPUT_PULLUP);
  50.   pinMode(FLIPPER_02, INPUT_PULLUP);
  51.   pinMode(revButton, INPUT_PULLUP);
  52.   setupLeds();
  53.   //BEGIN ODRIVE DEPENDECIES
  54.  
  55.     odrive_serial.begin(115200);  //Odrive Serial port
  56.       Serial.begin(115200);       //arduino - pc serial
  57.       while (!Serial) ; // wait for Arduino Serial Monitor to open
  58.  
  59.       Serial.println("ODriveArduino");
  60.       Serial.println("Setting parameters...");
  61.    for (int axis = 0; axis < 2; ++axis) {
  62.     odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 8500.0f << '\n'; //FeedRate Limit
  63.     odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 6.0f << '\n';       //Current Limit
  64.    }
  65.   Serial.println("Send the character '0' or '1' to calibrate respective motor");
  66.   Serial.println("Send the character '+' or '-' to shift gears");
  67.  
  68.   //calibrateO(); //Calibrate Motor 0
  69.   //calibrate1(); //Calibrate Motor 1
  70.   //END ODRIVE DEPENDECIES
  71.   // put your setup code here, to run once:
  72.   ledFullOn();
  73.   ledZero();
  74.   //GO_TO_GEAR = 1;
  75. }
  76.  
  77. void loop() {
  78.   // put your main code here, to run repeatedly:
  79.   testMoves();
  80.   shiftme();
  81.   readFlipperOne();
  82.   readFlipperTwo();
  83.   int Read_Rev_Button = digitalRead(revButton);
  84.   if(Read_Rev_Button == !NC)
  85.   {
  86.     digitalWrite(led5, HIGH);
  87.   }
  88.   else if(Read_Rev_Button == NC)
  89.   {
  90.     digitalWrite(led5, LOW);
  91.   }
  92. }
  93.  
  94. void readFlipperOne ()
  95. {
  96.   int READ_FLIPPER_1 = digitalRead(FLIPPER_01);
  97.   if(READ_FLIPPER_1 != LAST_FLIPPER_1_STATE)
  98.   {
  99.     lastDebounceTime_1 = millis();
  100.   }
  101.   if ((millis() - lastDebounceTime_1) > debounceDelay_1) {
  102.     if (READ_FLIPPER_1 != BUTTON_FLIPPER_1_STATE)
  103.     {
  104.       BUTTON_FLIPPER_1_STATE = READ_FLIPPER_1;
  105.       if (BUTTON_FLIPPER_1_STATE == NC && GO_TO_GEAR < 7)
  106.       {
  107.         //Serial.println("Button 1 did something");
  108.           GO_TO_GEAR += 1;
  109.           IsShifted = false;
  110.           FLIPPER_1_STATE = !FLIPPER_1_STATE;
  111.       }
  112.  
  113.      }
  114.   }
  115.     LAST_FLIPPER_1_STATE = READ_FLIPPER_1;
  116. }
  117.  
  118. void readFlipperTwo ()
  119. {
  120.   int Read_Rev_Button = digitalRead(revButton);
  121.   int READ_FLIPPER_2 = digitalRead(FLIPPER_02);
  122.   if(READ_FLIPPER_2 != LAST_FLIPPER_2_STATE)
  123.   {
  124.     lastDebounceTime_2 = millis();
  125.   }
  126.   if ((millis() - lastDebounceTime_2) > debounceDelay_2) {
  127.     if (READ_FLIPPER_2 != BUTTON_FLIPPER_2_STATE)
  128.     {
  129.       BUTTON_FLIPPER_2_STATE = READ_FLIPPER_2;
  130.       if (BUTTON_FLIPPER_2_STATE == NC)
  131.       {
  132.         //Serial.println("Button 1 did something");
  133.           if(GO_TO_GEAR > 1)
  134.           {
  135.             GO_TO_GEAR -= 1;
  136.             IsShifted = false;
  137.             FLIPPER_2_STATE = !FLIPPER_2_STATE;
  138.           }
  139.           else if(Read_Rev_Button == !NC && GO_TO_GEAR > 0)
  140.           {
  141.             GO_TO_GEAR -= 1;
  142.             reverse = true;
  143.             IsShifted = false;
  144.             FLIPPER_2_STATE = !FLIPPER_2_STATE;
  145.           }
  146.       }
  147.  
  148.      }
  149.   }
  150.     LAST_FLIPPER_2_STATE = READ_FLIPPER_2;
  151. }
  152.  
  153.  
  154.  
  155.  
  156. ||Calibrate.h
  157.  
  158.  
  159. void calibrateO()
  160. {
  161. int requested_state;
  162.     requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
  163.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  164.     odrive.run_state(atoi(0), requested_state, true);
  165.  
  166.     requested_state = ODriveArduino::AXIS_STATE_ENCODER_INDEX_SEARCH;
  167.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  168.     odrive.run_state(atoi(0), requested_state, true);
  169.  
  170.     requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL ;
  171.     Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n';
  172.     odrive.run_state(atoi(0), requested_state, false); // don't wait
  173.  
  174. //    odrive.save_configuration();
  175.     delay(5);
  176.     Serial.println("Saved Config");
  177. }
  178. void calibrate1()
  179. {
  180. int requested_state;
  181.     requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
  182.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  183.     odrive.run_state(atoi(0), requested_state, true);
  184.  
  185.     requested_state = ODriveArduino::AXIS_STATE_ENCODER_INDEX_SEARCH;
  186.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  187.     odrive.run_state(atoi(0), requested_state, true);
  188.  
  189.     requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL ;
  190.     Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n';
  191.     odrive.run_state(atoi(0), requested_state, false); // don't wait
  192.  
  193.     //odrive.save_configuration();
  194.     delay(5);
  195.     Serial.println("Saved Config");
  196. }
  197. void testMoves()
  198. {
  199.   if (Serial.available())
  200.   {
  201.     char c = Serial.read();
  202.     if (c == '0')
  203.     {
  204.        calibrateO();
  205.     }
  206.     if (c == '+')
  207.     {
  208.       //odrive.SetPosition(0, 1000);
  209.       if(GO_TO_GEAR < 7) {GO_TO_GEAR +=1;  IsShifted = false; Serial.println(GO_TO_GEAR);}    
  210.     }
  211.     if (c == '-')
  212.     {
  213.       //odrive.SetPosition(0, 0);
  214.       if(GO_TO_GEAR > 0) {GO_TO_GEAR -=1;  IsShifted = false;}  
  215.     }
  216.     if (c == '2')
  217.     {
  218.       odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n";
  219.           Serial << odrive.readFloat() << '\t';  
  220.     }
  221.    
  222.   }
  223. }
  224.  
  225. ||shifts.h
  226.  
  227.  
  228. #define bar_01_shiftPos_01 1000
  229. #define bar_02_shiftPos_01 1000
  230. #define bar_01_shiftPos_02 2000
  231. #define bar_02_shiftPos_02 2000
  232. #define bar_01_shiftPos_03 3000
  233. #define bar_02_shiftPos_03 3000
  234. #define bar_01_shiftPos_04 4000
  235. #define bar_02_shiftPos_04 4000
  236. #define bar_01_shiftPos_05 5000
  237. #define bar_02_shiftPos_05 5000
  238. #define bar_01_shiftPos_06 6000
  239. #define bar_02_shiftPos_06 6000
  240. #define bar_01_shiftPos_07 7000
  241. #define bar_02_shiftPos_07 7000
  242. #define bar_01_shiftPos_08 8000
  243. #define bar_02_shiftPos_08 8000
  244. int shift = 1;
  245.  
  246. void shiftme()
  247. {
  248.   shift = GO_TO_GEAR;
  249. if (reverse && !IsShifted) {
  250.  
  251.     Serial.println("Moving to gear Reverse");
  252.   ledA();
  253.   odrive.SetPosition(0, 0);
  254.   //odrive.SetPosition(1, bar_02_shiftPos_01);
  255.   SetStepperPos(10);
  256.   IsShifted = true;
  257.   reverse = false;
  258. }
  259. else if (shift == 1 && !IsShifted) {
  260.  
  261.     Serial.println("Moving to gear Neutral");
  262.   ledZero();
  263.   odrive.SetPosition(0, 1000);
  264.   //odrive.SetPosition(1, bar_02_shiftPos_02);
  265.   SetStepperPos(20);
  266.     IsShifted = true;
  267.   shift = -1;
  268.  
  269. }
  270. else if (shift == 2 && !IsShifted) {
  271.  
  272.     Serial.println("Moving to gear 1");
  273.   ledOne();
  274.   odrive.SetPosition(0, 2000);
  275.   //odrive.SetPosition(1, bar_02_shiftPos_03);
  276.   SetStepperPos(30);
  277.   shift = -1;
  278.   IsShifted = true;
  279. }
  280. else if (shift == 3 && !IsShifted) {
  281.  
  282.     Serial.println("Moving to gear 2");
  283.   ledTwo();
  284.   odrive.SetPosition(0, 3000);
  285.   //odrive.SetPosition(1, bar_02_shiftPos_04);
  286.   SetStepperPos(40);
  287.   shift = -1;
  288.   IsShifted = true;
  289. }
  290. else if (shift == 4 && !IsShifted) {
  291.  
  292.     Serial.println("Moving to gear 3");
  293.   ledThree();
  294.   odrive.SetPosition(0, 4000);
  295.   //odrive.SetPosition(1, bar_02_shiftPos_05);
  296.   SetStepperPos(50);
  297.   shift = -1;
  298.   IsShifted = true;
  299. }
  300. else if (shift == 5 && !IsShifted) {
  301.  
  302.     Serial.println("Moving to gear 4");
  303.   ledFour();
  304.   odrive.SetPosition(0, 5000);
  305.   //odrive.SetPosition(1, bar_02_shiftPos_06);
  306.   SetStepperPos(60);
  307.   shift = -1;
  308.   IsShifted = true;
  309. }
  310. else if (shift == 6 && !IsShifted) {
  311.  
  312.     Serial.println("Moving to gear 5");
  313.   ledFive();
  314.   odrive.SetPosition(0, 6000);
  315.   //odrive.SetPosition(1, bar_02_shiftPos_07);
  316.   SetStepperPos(70);
  317.   shift = -1;
  318.   IsShifted = true;
  319. }
  320. else if (shift == 7 && !IsShifted) {
  321.  
  322.     Serial.println("Moving to gear 6");
  323.   ledSix();
  324.   odrive.SetPosition(0, bar_01_shiftPos_08);
  325.   //odrive.SetPosition(1, bar_02_shiftPos_08);
  326.   SetStepperPos(80);
  327.   shift = -1;
  328.   IsShifted = true;
  329. }
  330. }
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!