main.cpp

00001 #include "mbed.h"
00002 /* 
00003   Example of Program for the MonsterMoto Shield on the ST Nucleo F401RE
00004   Code by : Didier Donsez
00005   
00006   Based on the Arduino sketch example coded by: Jim Lindblom,  SparkFun Electronics
00007  
00008  License: CC-SA 3.0, feel free to use this code however you'd like.
00009  Please improve upon it! Let me know how you've made it better.
00010  
00011  This is really simple example code to get you some basic
00012  functionality with the MonsterMoto Shield. The MonsterMote uses
00013  two VNH2SP30 high-current full-bridge motor drivers.
00014  */
00015 
00016 #define LOW 0
00017 #define HIGH 1
00018 
00019 DigitalOut statpin(D13, LOW); 
00020 
00021 #define MAXSPEED 1.0f
00022 
00023 #define BRAKEVCC 0
00024 #define CW   1
00025 #define CCW  2
00026 #define BRAKEGND 3
00027 #define CS_THRESHOLD 0.5f
00028 
00029 
00030 /*  VNH2SP30 pin definitions */
00031 DigitalOut inLeftApin(D7, LOW);  // INA: Clockwise input
00032 DigitalOut inRightApin(D4, LOW);  // INA: Clockwise input
00033 DigitalOut inLeftBpin(D8, LOW); // INB: Counter-clockwise input
00034 DigitalOut inRightBpin(D9, LOW); // INB: Counter-clockwise input
00035 PwmOut pwmLeftpin(PB_4); // PWM input
00036 PwmOut pwmRightpin(PB_10); // PWM input
00037 AnalogIn csLeftpin(A2); // CS: Current sense ANALOG input
00038 AnalogIn csRightpin(A3); // CS: Current sense ANALOG input
00039 AnalogIn enLeftpin(A0); // EN: Status of switches output (Analog pin)
00040 AnalogIn enRightpin(A1); // EN: Status of switches output (Analog pin)
00041 
00042 
00043 void setupShield()
00044 {  
00045     pwmLeftpin.period_ms(10);
00046     pwmLeftpin.pulsewidth_ms(1);
00047     pwmLeftpin.write(0.0f);
00048 
00049     pwmRightpin.period_ms(10);
00050     pwmRightpin.pulsewidth_ms(1);  
00051     pwmRightpin.write(0.0f);        
00052 }
00053 
00054 void checkShield()
00055 {  
00056   if ((csLeftpin.read_u16() < CS_THRESHOLD) && (csRightpin.read_u16() < CS_THRESHOLD))
00057     statpin.write(HIGH);
00058   else 
00059       statpin.write(LOW);
00060 }
00061 
00062 
00063 void stopLeftMotor()
00064 {
00065         inLeftApin.write(LOW);
00066         inLeftBpin.write(LOW);
00067         pwmLeftpin.write(0.0f);
00068 }
00069 
00070 void stopRightMotor()
00071 {
00072         inRightApin.write(LOW);
00073         inRightBpin.write(LOW);
00074         pwmRightpin.write(0.0f);
00075 }
00076 
00077 /* 
00078  set a motor going in a specific direction
00079  the motor will continue going in that direction, at that speed
00080  until told to do otherwise.
00081  
00082  direct: Should be between 0 and 3, with the following result
00083  0: Brake to VCC
00084  1: Clockwise
00085  2: CounterClockwise
00086  3: Brake to GND
00087  
00088 BRAKEVCC 0
00089 CW   1
00090 CCW  2
00091 BRAKEGND 3
00092  
00093 pwm: should be a value between 0.0f and 1.0f, higher the number, the faster it'll go
00094  */
00095  
00096 void goLeftMotor(uint8_t direct, float percent)
00097 {
00098     if (direct <=4)
00099     {
00100       if (direct <=1)
00101         inLeftApin.write(HIGH);
00102       else
00103         inLeftApin.write(LOW);
00104 
00105       if ((direct==0)||(direct==2))
00106         inLeftBpin.write(HIGH);
00107       else
00108         inLeftBpin.write(LOW);
00109 
00110         pwmLeftpin.write(percent);
00111     }
00112 }
00113  
00114 void goRightMotor(uint8_t direct, float percent)
00115 {
00116     if (direct <=4)
00117     {
00118       if (direct <=1)
00119         inRightApin.write(HIGH);
00120       else
00121         inRightApin.write(LOW);
00122 
00123       if ((direct==0)||(direct==2))
00124         inRightBpin.write(HIGH);
00125       else
00126         inRightBpin.write(LOW);
00127 
00128         pwmRightpin.write(percent);
00129     }
00130 }
00131  
00132 
00133 void setup()
00134 {  
00135     setupShield();
00136 }
00137 
00138 void loop()
00139 {
00140   goLeftMotor(CW, MAXSPEED);
00141   goRightMotor(CCW, MAXSPEED);
00142   checkShield();
00143   wait_ms(5000);
00144 
00145   stopLeftMotor();
00146   stopRightMotor();
00147   checkShield();
00148   wait_ms(2000);
00149 
00150   goLeftMotor(CCW, MAXSPEED);
00151   goRightMotor(CW, MAXSPEED);
00152   checkShield();
00153   wait_ms(5000);
00154 
00155   stopLeftMotor();
00156   stopRightMotor();
00157   checkShield();
00158   wait_ms(2000);
00159 }
00160 
00161 int main() {
00162     setup();
00163     while(1) {
00164         loop();
00165     }
00166 }
00167