Diff: main.cpp

Revision:

0:50670948e4d6

Child:

1:ec9cd1ae6f86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 01 21:44:44 2014 +0000
@@ -0,0 +1,167 @@
+#include "mbed.h"
+/* 
+  Example of Program for the MonsterMoto Shield on the ST Nucleo F401RE
+  Code by : Didier Donsez
+  
+  Based on the Arduino sketch example coded by: Jim Lindblom,  SparkFun Electronics
+ 
+ License: CC-SA 3.0, feel free to use this code however you'd like.
+ Please improve upon it! Let me know how you've made it better.
+ 
+ This is really simple example code to get you some basic
+ functionality with the MonsterMoto Shield. The MonsterMote uses
+ two VNH2SP30 high-current full-bridge motor drivers.
+ */
+
+#define LOW 0
+#define HIGH 1
+
+DigitalOut statpin(D13, LOW); 
+
+#define MAXSPEED 1.0f
+
+#define BRAKEVCC 0
+#define CW   1
+#define CCW  2
+#define BRAKEGND 3
+#define CS_THRESHOLD 0.5f
+
+
+/*  VNH2SP30 pin definitions */
+DigitalOut inLeftApin(D7, LOW);  // INA: Clockwise input
+DigitalOut inRightApin(D4, LOW);  // INA: Clockwise input
+DigitalOut inLeftBpin(D8, LOW); // INB: Counter-clockwise input
+DigitalOut inRightBpin(D9, LOW); // INB: Counter-clockwise input
+PwmOut pwmLeftpin(PB_4); // PWM input
+PwmOut pwmRightpin(PB_10); // PWM input
+AnalogIn csLeftpin(A2); // CS: Current sense ANALOG input
+AnalogIn csRightpin(A3); // CS: Current sense ANALOG input
+AnalogIn enLeftpin(A0); // EN: Status of switches output (Analog pin)
+AnalogIn enRightpin(A1); // EN: Status of switches output (Analog pin)
+
+
+void setupShield()
+{  
+    pwmLeftpin.period_ms(10);
+    pwmLeftpin.pulsewidth_ms(1);
+    pwmLeftpin.write(0.0f);
+
+    pwmRightpin.period_ms(10);
+    pwmRightpin.pulsewidth_ms(1);  
+    pwmRightpin.write(0.0f);        
+}
+
+void checkShield()
+{  
+  if ((csLeftpin.read_u16() < CS_THRESHOLD) && (csRightpin.read_u16() < CS_THRESHOLD))
+    statpin.write(HIGH);
+  else 
+      statpin.write(LOW);
+}
+
+
+void stopLeftMotor()
+{
+        inLeftApin.write(LOW);
+        inLeftBpin.write(LOW);
+        pwmLeftpin.write(0.0f);
+}
+
+void stopRightMotor()
+{
+        inRightApin.write(LOW);
+        inRightBpin.write(LOW);
+        pwmRightpin.write(0.0f);
+}
+
+/* 
+ set a motor going in a specific direction
+ the motor will continue going in that direction, at that speed
+ until told to do otherwise.
+ 
+ direct: Should be between 0 and 3, with the following result
+ 0: Brake to VCC
+ 1: Clockwise
+ 2: CounterClockwise
+ 3: Brake to GND
+ 
+BRAKEVCC 0
+CW   1
+CCW  2
+BRAKEGND 3
+ 
+pwm: should be a value between 0.0f and 1.0f, higher the number, the faster it'll go
+ */
+ 
+void goLeftMotor(uint8_t direct, float percent)
+{
+    if (direct <=4)
+    {
+      if (direct <=1)
+        inLeftApin.write(HIGH);
+      else
+        inLeftApin.write(LOW);
+
+      if ((direct==0)||(direct==2))
+        inLeftBpin.write(HIGH);
+      else
+        inLeftBpin.write(LOW);
+
+        pwmLeftpin.write(percent);
+    }
+}
+ 
+void goRightMotor(uint8_t direct, float percent)
+{
+    if (direct <=4)
+    {
+      if (direct <=1)
+        inRightApin.write(HIGH);
+      else
+        inRightApin.write(LOW);
+
+      if ((direct==0)||(direct==2))
+        inRightBpin.write(HIGH);
+      else
+        inRightBpin.write(LOW);
+
+        pwmRightpin.write(percent);
+    }
+}
+ 
+
+void setup()
+{  
+    setupShield();
+}
+
+void loop()
+{
+  goLeftMotor(CW, MAXSPEED);
+  goRightMotor(CCW, MAXSPEED);
+  checkShield();
+  wait_ms(5000);
+
+  stopLeftMotor();
+  stopRightMotor();
+  checkShield();
+  wait_ms(2000);
+
+  goLeftMotor(CCW, MAXSPEED);
+  goRightMotor(CW, MAXSPEED);
+  checkShield();
+  wait_ms(5000);
+
+  stopLeftMotor();
+  stopRightMotor();
+  checkShield();
+  wait_ms(2000);
+}
+
+int main() {
+    setup();
+    while(1) {
+        loop();
+    }
+}
+ 
\ No newline at end of file