Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Public Pastes
Guest

linak DL2 code

By: a guest on Oct 11th, 2014  |  syntax: None  |  size: 4.63 KB  |  views: 210  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. #include "DualVNH5019MotorShield.h"
  2.  
  3. DualVNH5019MotorShield md;
  4.  
  5. //these speed variables are the ones that you need to set yourself
  6. //through experimentation based on your specific motors in your DL2.
  7. const int upSpeedA = 360;
  8. const int downSpeedA = -360;
  9. const int upSpeedB = 378; // must be positive < 400
  10. const int downSpeedB = -362;  // must be negative < -400
  11.  
  12. const int A = 0;
  13. const int B = 1;
  14. const int AB = 3;
  15.  
  16. const int NOSTOP = 0;
  17. const int STOP = 1;
  18.  
  19. const int downButtonPin = 3;
  20. const int upButtonPin = 5;
  21. const int STEP = 10;
  22.  
  23. void stopIfFault()
  24. {
  25.   if (md.getM1Fault())
  26.   {
  27.     Serial.println("M1 fault");
  28.     while(1);
  29.   }
  30.   if (md.getM2Fault())
  31.   {
  32.     Serial.println("M2 fault");
  33.     while(1);
  34.   }
  35. }
  36.  
  37. void up(int motor, int msec = 0, int stopmotor = STOP) {
  38.   int upspeed;
  39.   if(motor == A) {
  40.     upspeed = upSpeedA;
  41.     Serial.print("A going UP.\n");
  42.       if(stopmotor == NOSTOP) {
  43.        Serial.print("  and not stopping.\n");
  44.       }  
  45.   }
  46.   if(motor == B) {
  47.     upspeed = upSpeedB;
  48.     Serial.print("B going UP.\n");
  49.       if(stopmotor == NOSTOP) {
  50.        Serial.print("  and not stopping.\n");
  51.       }
  52.   }
  53.   if(motor == AB) {
  54.     if (upSpeedB <= upSpeedA) {
  55.       upspeed = upSpeedA;
  56.     } else {
  57.       upspeed = upSpeedB;
  58.     }
  59.     Serial.print("A and B going UP.\n");
  60.       if(stopmotor == NOSTOP) {
  61.        Serial.print("  and not stopping.\n");
  62.       }
  63.   }
  64.  
  65.   for (int i = 0; i <= upspeed; i+=STEP)
  66.   {
  67.     if (motor == A) md.setM1Speed(i);
  68.     if (motor == B) md.setM2Speed(i);
  69.     if (motor == AB) {
  70.       if (i <= upSpeedA) md.setM1Speed(i);
  71.       if (i <= upSpeedB) md.setM2Speed(i);
  72.     }
  73.     stopIfFault();
  74.     delay(2);
  75.   }
  76.  
  77.   if (msec > 0) delay(msec);
  78.  
  79.   if (motor == A || motor == AB) {
  80.     Serial.print("A current: ");
  81.     Serial.println(md.getM1CurrentMilliamps());
  82.     if (stopmotor == STOP) md.setM1Speed(0);
  83.   }  
  84.   if (motor == B || motor == AB) {
  85.     Serial.print("B current: ");
  86.     Serial.println(md.getM2CurrentMilliamps());
  87.     if (stopmotor == STOP) md.setM2Speed(0);
  88.   }
  89. }
  90.  
  91. void down(int motor, int msec = 0, int stopmotor = STOP) {
  92.   int downspeed;
  93.   if(motor == A) {
  94.     downspeed = downSpeedA;
  95.     Serial.print("A going DOWN\n");
  96.       if(stopmotor == NOSTOP) {
  97.        Serial.print("  and not stopping.\n");
  98.       }  
  99.   }
  100.   if(motor == B) {
  101.     downspeed = downSpeedB;
  102.     Serial.print("B going UP\n");
  103.       if(stopmotor == NOSTOP) {
  104.        Serial.print("  and not stopping.\n");
  105.       }
  106.   }
  107.   if(motor == AB) {
  108.     if (downSpeedB <= downSpeedA) {
  109.       downspeed = downSpeedB;
  110.     } else {
  111.       downspeed = downSpeedA;
  112.     }
  113.     Serial.print("A and B going DOWN.\n");
  114.       if(stopmotor == NOSTOP) {
  115.        Serial.print("  and not stopping.\n");
  116.       }
  117.   }
  118.  
  119.   for (int i = 0; i >= downspeed; i-=STEP)
  120.   {
  121.     if (motor == A) md.setM1Speed(i);
  122.     if (motor == B) md.setM2Speed(i);
  123.     if (motor == AB) {
  124.       if (i >= downSpeedA) md.setM1Speed(i);
  125.       if (i >= downSpeedB) md.setM2Speed(i);
  126.     }
  127.     stopIfFault();
  128.     delay(2);
  129.   }
  130.  
  131.   if (msec > 0) delay(msec);
  132.  
  133.   if (motor == A || motor == AB) {
  134.     Serial.print("A current: ");
  135.     Serial.println(md.getM1CurrentMilliamps());
  136.     if (stopmotor == STOP) md.setM1Speed(0);
  137.   }  
  138.   if (motor == B || motor == AB) {
  139.     Serial.print("B current: ");
  140.     Serial.println(md.getM2CurrentMilliamps());
  141.     if (stopmotor == STOP) md.setM2Speed(0);
  142.   }
  143. }
  144.  
  145.  
  146. void stop(int motor) {
  147.   if(motor == A) md.setM1Brake(0);
  148.   if(motor == B) md.setM2Brake(0);
  149.   if(motor == AB) md.setBrakes(0,0);
  150. }
  151.  
  152. int donealready = 0;
  153.  
  154. void manualMode() {
  155.   int downButtonState;
  156.   int upButtonState;
  157.  
  158.   while(1) {
  159.     downButtonState = digitalRead(downButtonPin);
  160.     if (downButtonState == HIGH) {
  161.         Serial.print("downButton is pressed.\n");
  162.         down(AB,5);
  163.     }
  164.     int upButtonState = digitalRead(upButtonPin);
  165.     if (upButtonState == HIGH) {
  166.         Serial.print("upButton is pressed.\n");
  167.         up(AB,5);
  168.     }
  169. //    delay();
  170.  }
  171. }
  172.  
  173. // the time and downfactor are very sensitive variables
  174. // you must determine and set for your own setup.
  175. int time = 11850;
  176. float downfactor = 1.012;
  177.  
  178. void exerciseMode() {
  179.   delay(60000);
  180.   up(AB, 0, NOSTOP);
  181.   delay(time);
  182.   stop(AB);
  183.   delay(60000);
  184.   down(AB, 0, NOSTOP);
  185.   delay(time*downfactor);
  186.   stop(AB);
  187.   delay(60000);
  188. }
  189. void setup()
  190. {
  191.   Serial.begin(115200);
  192.   Serial.println("Dual VNH5019 Motor Shield starting in 5 seconds\n");
  193.   pinMode(downButtonPin, INPUT);
  194.   pinMode(upButtonPin, INPUT);
  195.  
  196.   md.init();
  197.   delay(5000);
  198. }
  199.  
  200.  
  201. void loop()
  202. {  
  203.  //manualMode();
  204.  exerciseMode();
  205.  
  206. }
clone this paste RAW Paste Data