Example of Program for the MonsterMoto Shield on the ST Nucleo L152RE
Fork of NucleoF401_MonsterMotoShield by
main.cpp@0:50670948e4d6, 2014-08-01 (annotated)
- Committer:
- donsez
- Date:
- Fri Aug 01 21:44:44 2014 +0000
- Revision:
- 0:50670948e4d6
- Child:
- 1:ec9cd1ae6f86
Initial commit of the example for MonsterMotoShield;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
donsez | 0:50670948e4d6 | 1 | #include "mbed.h" |
donsez | 0:50670948e4d6 | 2 | /* |
donsez | 0:50670948e4d6 | 3 | Example of Program for the MonsterMoto Shield on the ST Nucleo F401RE |
donsez | 0:50670948e4d6 | 4 | Code by : Didier Donsez |
donsez | 0:50670948e4d6 | 5 | |
donsez | 0:50670948e4d6 | 6 | Based on the Arduino sketch example coded by: Jim Lindblom, SparkFun Electronics |
donsez | 0:50670948e4d6 | 7 | |
donsez | 0:50670948e4d6 | 8 | License: CC-SA 3.0, feel free to use this code however you'd like. |
donsez | 0:50670948e4d6 | 9 | Please improve upon it! Let me know how you've made it better. |
donsez | 0:50670948e4d6 | 10 | |
donsez | 0:50670948e4d6 | 11 | This is really simple example code to get you some basic |
donsez | 0:50670948e4d6 | 12 | functionality with the MonsterMoto Shield. The MonsterMote uses |
donsez | 0:50670948e4d6 | 13 | two VNH2SP30 high-current full-bridge motor drivers. |
donsez | 0:50670948e4d6 | 14 | */ |
donsez | 0:50670948e4d6 | 15 | |
donsez | 0:50670948e4d6 | 16 | #define LOW 0 |
donsez | 0:50670948e4d6 | 17 | #define HIGH 1 |
donsez | 0:50670948e4d6 | 18 | |
donsez | 0:50670948e4d6 | 19 | DigitalOut statpin(D13, LOW); |
donsez | 0:50670948e4d6 | 20 | |
donsez | 0:50670948e4d6 | 21 | #define MAXSPEED 1.0f |
donsez | 0:50670948e4d6 | 22 | |
donsez | 0:50670948e4d6 | 23 | #define BRAKEVCC 0 |
donsez | 0:50670948e4d6 | 24 | #define CW 1 |
donsez | 0:50670948e4d6 | 25 | #define CCW 2 |
donsez | 0:50670948e4d6 | 26 | #define BRAKEGND 3 |
donsez | 0:50670948e4d6 | 27 | #define CS_THRESHOLD 0.5f |
donsez | 0:50670948e4d6 | 28 | |
donsez | 0:50670948e4d6 | 29 | |
donsez | 0:50670948e4d6 | 30 | /* VNH2SP30 pin definitions */ |
donsez | 0:50670948e4d6 | 31 | DigitalOut inLeftApin(D7, LOW); // INA: Clockwise input |
donsez | 0:50670948e4d6 | 32 | DigitalOut inRightApin(D4, LOW); // INA: Clockwise input |
donsez | 0:50670948e4d6 | 33 | DigitalOut inLeftBpin(D8, LOW); // INB: Counter-clockwise input |
donsez | 0:50670948e4d6 | 34 | DigitalOut inRightBpin(D9, LOW); // INB: Counter-clockwise input |
donsez | 0:50670948e4d6 | 35 | PwmOut pwmLeftpin(PB_4); // PWM input |
donsez | 0:50670948e4d6 | 36 | PwmOut pwmRightpin(PB_10); // PWM input |
donsez | 0:50670948e4d6 | 37 | AnalogIn csLeftpin(A2); // CS: Current sense ANALOG input |
donsez | 0:50670948e4d6 | 38 | AnalogIn csRightpin(A3); // CS: Current sense ANALOG input |
donsez | 0:50670948e4d6 | 39 | AnalogIn enLeftpin(A0); // EN: Status of switches output (Analog pin) |
donsez | 0:50670948e4d6 | 40 | AnalogIn enRightpin(A1); // EN: Status of switches output (Analog pin) |
donsez | 0:50670948e4d6 | 41 | |
donsez | 0:50670948e4d6 | 42 | |
donsez | 0:50670948e4d6 | 43 | void setupShield() |
donsez | 0:50670948e4d6 | 44 | { |
donsez | 0:50670948e4d6 | 45 | pwmLeftpin.period_ms(10); |
donsez | 0:50670948e4d6 | 46 | pwmLeftpin.pulsewidth_ms(1); |
donsez | 0:50670948e4d6 | 47 | pwmLeftpin.write(0.0f); |
donsez | 0:50670948e4d6 | 48 | |
donsez | 0:50670948e4d6 | 49 | pwmRightpin.period_ms(10); |
donsez | 0:50670948e4d6 | 50 | pwmRightpin.pulsewidth_ms(1); |
donsez | 0:50670948e4d6 | 51 | pwmRightpin.write(0.0f); |
donsez | 0:50670948e4d6 | 52 | } |
donsez | 0:50670948e4d6 | 53 | |
donsez | 0:50670948e4d6 | 54 | void checkShield() |
donsez | 0:50670948e4d6 | 55 | { |
donsez | 0:50670948e4d6 | 56 | if ((csLeftpin.read_u16() < CS_THRESHOLD) && (csRightpin.read_u16() < CS_THRESHOLD)) |
donsez | 0:50670948e4d6 | 57 | statpin.write(HIGH); |
donsez | 0:50670948e4d6 | 58 | else |
donsez | 0:50670948e4d6 | 59 | statpin.write(LOW); |
donsez | 0:50670948e4d6 | 60 | } |
donsez | 0:50670948e4d6 | 61 | |
donsez | 0:50670948e4d6 | 62 | |
donsez | 0:50670948e4d6 | 63 | void stopLeftMotor() |
donsez | 0:50670948e4d6 | 64 | { |
donsez | 0:50670948e4d6 | 65 | inLeftApin.write(LOW); |
donsez | 0:50670948e4d6 | 66 | inLeftBpin.write(LOW); |
donsez | 0:50670948e4d6 | 67 | pwmLeftpin.write(0.0f); |
donsez | 0:50670948e4d6 | 68 | } |
donsez | 0:50670948e4d6 | 69 | |
donsez | 0:50670948e4d6 | 70 | void stopRightMotor() |
donsez | 0:50670948e4d6 | 71 | { |
donsez | 0:50670948e4d6 | 72 | inRightApin.write(LOW); |
donsez | 0:50670948e4d6 | 73 | inRightBpin.write(LOW); |
donsez | 0:50670948e4d6 | 74 | pwmRightpin.write(0.0f); |
donsez | 0:50670948e4d6 | 75 | } |
donsez | 0:50670948e4d6 | 76 | |
donsez | 0:50670948e4d6 | 77 | /* |
donsez | 0:50670948e4d6 | 78 | set a motor going in a specific direction |
donsez | 0:50670948e4d6 | 79 | the motor will continue going in that direction, at that speed |
donsez | 0:50670948e4d6 | 80 | until told to do otherwise. |
donsez | 0:50670948e4d6 | 81 | |
donsez | 0:50670948e4d6 | 82 | direct: Should be between 0 and 3, with the following result |
donsez | 0:50670948e4d6 | 83 | 0: Brake to VCC |
donsez | 0:50670948e4d6 | 84 | 1: Clockwise |
donsez | 0:50670948e4d6 | 85 | 2: CounterClockwise |
donsez | 0:50670948e4d6 | 86 | 3: Brake to GND |
donsez | 0:50670948e4d6 | 87 | |
donsez | 0:50670948e4d6 | 88 | BRAKEVCC 0 |
donsez | 0:50670948e4d6 | 89 | CW 1 |
donsez | 0:50670948e4d6 | 90 | CCW 2 |
donsez | 0:50670948e4d6 | 91 | BRAKEGND 3 |
donsez | 0:50670948e4d6 | 92 | |
donsez | 0:50670948e4d6 | 93 | pwm: should be a value between 0.0f and 1.0f, higher the number, the faster it'll go |
donsez | 0:50670948e4d6 | 94 | */ |
donsez | 0:50670948e4d6 | 95 | |
donsez | 0:50670948e4d6 | 96 | void goLeftMotor(uint8_t direct, float percent) |
donsez | 0:50670948e4d6 | 97 | { |
donsez | 0:50670948e4d6 | 98 | if (direct <=4) |
donsez | 0:50670948e4d6 | 99 | { |
donsez | 0:50670948e4d6 | 100 | if (direct <=1) |
donsez | 0:50670948e4d6 | 101 | inLeftApin.write(HIGH); |
donsez | 0:50670948e4d6 | 102 | else |
donsez | 0:50670948e4d6 | 103 | inLeftApin.write(LOW); |
donsez | 0:50670948e4d6 | 104 | |
donsez | 0:50670948e4d6 | 105 | if ((direct==0)||(direct==2)) |
donsez | 0:50670948e4d6 | 106 | inLeftBpin.write(HIGH); |
donsez | 0:50670948e4d6 | 107 | else |
donsez | 0:50670948e4d6 | 108 | inLeftBpin.write(LOW); |
donsez | 0:50670948e4d6 | 109 | |
donsez | 0:50670948e4d6 | 110 | pwmLeftpin.write(percent); |
donsez | 0:50670948e4d6 | 111 | } |
donsez | 0:50670948e4d6 | 112 | } |
donsez | 0:50670948e4d6 | 113 | |
donsez | 0:50670948e4d6 | 114 | void goRightMotor(uint8_t direct, float percent) |
donsez | 0:50670948e4d6 | 115 | { |
donsez | 0:50670948e4d6 | 116 | if (direct <=4) |
donsez | 0:50670948e4d6 | 117 | { |
donsez | 0:50670948e4d6 | 118 | if (direct <=1) |
donsez | 0:50670948e4d6 | 119 | inRightApin.write(HIGH); |
donsez | 0:50670948e4d6 | 120 | else |
donsez | 0:50670948e4d6 | 121 | inRightApin.write(LOW); |
donsez | 0:50670948e4d6 | 122 | |
donsez | 0:50670948e4d6 | 123 | if ((direct==0)||(direct==2)) |
donsez | 0:50670948e4d6 | 124 | inRightBpin.write(HIGH); |
donsez | 0:50670948e4d6 | 125 | else |
donsez | 0:50670948e4d6 | 126 | inRightBpin.write(LOW); |
donsez | 0:50670948e4d6 | 127 | |
donsez | 0:50670948e4d6 | 128 | pwmRightpin.write(percent); |
donsez | 0:50670948e4d6 | 129 | } |
donsez | 0:50670948e4d6 | 130 | } |
donsez | 0:50670948e4d6 | 131 | |
donsez | 0:50670948e4d6 | 132 | |
donsez | 0:50670948e4d6 | 133 | void setup() |
donsez | 0:50670948e4d6 | 134 | { |
donsez | 0:50670948e4d6 | 135 | setupShield(); |
donsez | 0:50670948e4d6 | 136 | } |
donsez | 0:50670948e4d6 | 137 | |
donsez | 0:50670948e4d6 | 138 | void loop() |
donsez | 0:50670948e4d6 | 139 | { |
donsez | 0:50670948e4d6 | 140 | goLeftMotor(CW, MAXSPEED); |
donsez | 0:50670948e4d6 | 141 | goRightMotor(CCW, MAXSPEED); |
donsez | 0:50670948e4d6 | 142 | checkShield(); |
donsez | 0:50670948e4d6 | 143 | wait_ms(5000); |
donsez | 0:50670948e4d6 | 144 | |
donsez | 0:50670948e4d6 | 145 | stopLeftMotor(); |
donsez | 0:50670948e4d6 | 146 | stopRightMotor(); |
donsez | 0:50670948e4d6 | 147 | checkShield(); |
donsez | 0:50670948e4d6 | 148 | wait_ms(2000); |
donsez | 0:50670948e4d6 | 149 | |
donsez | 0:50670948e4d6 | 150 | goLeftMotor(CCW, MAXSPEED); |
donsez | 0:50670948e4d6 | 151 | goRightMotor(CW, MAXSPEED); |
donsez | 0:50670948e4d6 | 152 | checkShield(); |
donsez | 0:50670948e4d6 | 153 | wait_ms(5000); |
donsez | 0:50670948e4d6 | 154 | |
donsez | 0:50670948e4d6 | 155 | stopLeftMotor(); |
donsez | 0:50670948e4d6 | 156 | stopRightMotor(); |
donsez | 0:50670948e4d6 | 157 | checkShield(); |
donsez | 0:50670948e4d6 | 158 | wait_ms(2000); |
donsez | 0:50670948e4d6 | 159 | } |
donsez | 0:50670948e4d6 | 160 | |
donsez | 0:50670948e4d6 | 161 | int main() { |
donsez | 0:50670948e4d6 | 162 | setup(); |
donsez | 0:50670948e4d6 | 163 | while(1) { |
donsez | 0:50670948e4d6 | 164 | loop(); |
donsez | 0:50670948e4d6 | 165 | } |
donsez | 0:50670948e4d6 | 166 | } |
donsez | 0:50670948e4d6 | 167 |