Example of Program for the MonsterMoto Shield on the ST Nucleo L152RE
Fork of NucleoF401_MonsterMotoShield by
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