Example of Program for the MonsterMoto Shield on the ST Nucleo L152RE
Fork of NucleoF401_MonsterMotoShield by
main.cpp@1:ec9cd1ae6f86, 2015-02-05 (annotated)
- Committer:
- Michel_Vivien
- Date:
- Thu Feb 05 16:57:02 2015 +0000
- Revision:
- 1:ec9cd1ae6f86
- Parent:
- 0:50670948e4d6
- Child:
- 2:7f9d0d59c7f5
changement pin pour nucleo l1
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 */ |
Michel_Vivien | 1:ec9cd1ae6f86 | 31 | DigitalOut dirA(D12, LOW); // INA: Clockwise input |
Michel_Vivien | 1:ec9cd1ae6f86 | 32 | DigitalOut dirB(D13, LOW); // INB: Counter-clockwise input |
Michel_Vivien | 1:ec9cd1ae6f86 | 33 | PwmOut pwmLeftpin(PA_7); // PWM input |
Michel_Vivien | 1:ec9cd1ae6f86 | 34 | PwmOut pwmRightpin(PB_3); // PWM input |
donsez | 0:50670948e4d6 | 35 | AnalogIn csLeftpin(A2); // CS: Current sense ANALOG input |
donsez | 0:50670948e4d6 | 36 | AnalogIn csRightpin(A3); // CS: Current sense ANALOG input |
donsez | 0:50670948e4d6 | 37 | AnalogIn enLeftpin(A0); // EN: Status of switches output (Analog pin) |
donsez | 0:50670948e4d6 | 38 | AnalogIn enRightpin(A1); // EN: Status of switches output (Analog pin) |
donsez | 0:50670948e4d6 | 39 | |
donsez | 0:50670948e4d6 | 40 | |
donsez | 0:50670948e4d6 | 41 | void setupShield() |
donsez | 0:50670948e4d6 | 42 | { |
donsez | 0:50670948e4d6 | 43 | pwmLeftpin.period_ms(10); |
donsez | 0:50670948e4d6 | 44 | pwmLeftpin.pulsewidth_ms(1); |
donsez | 0:50670948e4d6 | 45 | pwmLeftpin.write(0.0f); |
donsez | 0:50670948e4d6 | 46 | |
donsez | 0:50670948e4d6 | 47 | pwmRightpin.period_ms(10); |
donsez | 0:50670948e4d6 | 48 | pwmRightpin.pulsewidth_ms(1); |
donsez | 0:50670948e4d6 | 49 | pwmRightpin.write(0.0f); |
donsez | 0:50670948e4d6 | 50 | } |
donsez | 0:50670948e4d6 | 51 | |
donsez | 0:50670948e4d6 | 52 | void checkShield() |
donsez | 0:50670948e4d6 | 53 | { |
donsez | 0:50670948e4d6 | 54 | if ((csLeftpin.read_u16() < CS_THRESHOLD) && (csRightpin.read_u16() < CS_THRESHOLD)) |
donsez | 0:50670948e4d6 | 55 | statpin.write(HIGH); |
donsez | 0:50670948e4d6 | 56 | else |
donsez | 0:50670948e4d6 | 57 | statpin.write(LOW); |
donsez | 0:50670948e4d6 | 58 | } |
donsez | 0:50670948e4d6 | 59 | |
donsez | 0:50670948e4d6 | 60 | |
donsez | 0:50670948e4d6 | 61 | void stopLeftMotor() |
donsez | 0:50670948e4d6 | 62 | { |
donsez | 0:50670948e4d6 | 63 | pwmLeftpin.write(0.0f); |
donsez | 0:50670948e4d6 | 64 | } |
donsez | 0:50670948e4d6 | 65 | |
donsez | 0:50670948e4d6 | 66 | void stopRightMotor() |
donsez | 0:50670948e4d6 | 67 | { |
donsez | 0:50670948e4d6 | 68 | pwmRightpin.write(0.0f); |
donsez | 0:50670948e4d6 | 69 | } |
donsez | 0:50670948e4d6 | 70 | |
donsez | 0:50670948e4d6 | 71 | /* |
donsez | 0:50670948e4d6 | 72 | set a motor going in a specific direction |
donsez | 0:50670948e4d6 | 73 | the motor will continue going in that direction, at that speed |
donsez | 0:50670948e4d6 | 74 | until told to do otherwise. |
donsez | 0:50670948e4d6 | 75 | |
donsez | 0:50670948e4d6 | 76 | direct: Should be between 0 and 3, with the following result |
donsez | 0:50670948e4d6 | 77 | 0: Brake to VCC |
donsez | 0:50670948e4d6 | 78 | 1: Clockwise |
donsez | 0:50670948e4d6 | 79 | 2: CounterClockwise |
donsez | 0:50670948e4d6 | 80 | 3: Brake to GND |
donsez | 0:50670948e4d6 | 81 | |
donsez | 0:50670948e4d6 | 82 | BRAKEVCC 0 |
donsez | 0:50670948e4d6 | 83 | CW 1 |
donsez | 0:50670948e4d6 | 84 | CCW 2 |
donsez | 0:50670948e4d6 | 85 | BRAKEGND 3 |
donsez | 0:50670948e4d6 | 86 | |
donsez | 0:50670948e4d6 | 87 | pwm: should be a value between 0.0f and 1.0f, higher the number, the faster it'll go |
donsez | 0:50670948e4d6 | 88 | */ |
donsez | 0:50670948e4d6 | 89 | |
donsez | 0:50670948e4d6 | 90 | void goLeftMotor(uint8_t direct, float percent) |
donsez | 0:50670948e4d6 | 91 | { |
donsez | 0:50670948e4d6 | 92 | if (direct <=4) |
donsez | 0:50670948e4d6 | 93 | { |
donsez | 0:50670948e4d6 | 94 | if (direct <=1) |
Michel_Vivien | 1:ec9cd1ae6f86 | 95 | dirA.write(HIGH); |
donsez | 0:50670948e4d6 | 96 | else |
Michel_Vivien | 1:ec9cd1ae6f86 | 97 | dirA.write(LOW); |
donsez | 0:50670948e4d6 | 98 | |
donsez | 0:50670948e4d6 | 99 | if ((direct==0)||(direct==2)) |
Michel_Vivien | 1:ec9cd1ae6f86 | 100 | dirB.write(HIGH); |
donsez | 0:50670948e4d6 | 101 | else |
Michel_Vivien | 1:ec9cd1ae6f86 | 102 | dirB.write(LOW); |
donsez | 0:50670948e4d6 | 103 | |
donsez | 0:50670948e4d6 | 104 | pwmLeftpin.write(percent); |
donsez | 0:50670948e4d6 | 105 | } |
donsez | 0:50670948e4d6 | 106 | } |
donsez | 0:50670948e4d6 | 107 | |
donsez | 0:50670948e4d6 | 108 | void goRightMotor(uint8_t direct, float percent) |
donsez | 0:50670948e4d6 | 109 | { |
donsez | 0:50670948e4d6 | 110 | if (direct <=4) |
donsez | 0:50670948e4d6 | 111 | { |
donsez | 0:50670948e4d6 | 112 | if (direct <=1) |
Michel_Vivien | 1:ec9cd1ae6f86 | 113 | dirA.write(HIGH); |
donsez | 0:50670948e4d6 | 114 | else |
Michel_Vivien | 1:ec9cd1ae6f86 | 115 | dirA.write(LOW); |
donsez | 0:50670948e4d6 | 116 | |
donsez | 0:50670948e4d6 | 117 | if ((direct==0)||(direct==2)) |
Michel_Vivien | 1:ec9cd1ae6f86 | 118 | dirB.write(HIGH); |
donsez | 0:50670948e4d6 | 119 | else |
Michel_Vivien | 1:ec9cd1ae6f86 | 120 | dirB.write(LOW); |
donsez | 0:50670948e4d6 | 121 | |
donsez | 0:50670948e4d6 | 122 | pwmRightpin.write(percent); |
donsez | 0:50670948e4d6 | 123 | } |
donsez | 0:50670948e4d6 | 124 | } |
donsez | 0:50670948e4d6 | 125 | |
donsez | 0:50670948e4d6 | 126 | |
donsez | 0:50670948e4d6 | 127 | void setup() |
donsez | 0:50670948e4d6 | 128 | { |
donsez | 0:50670948e4d6 | 129 | setupShield(); |
donsez | 0:50670948e4d6 | 130 | } |
donsez | 0:50670948e4d6 | 131 | |
donsez | 0:50670948e4d6 | 132 | void loop() |
donsez | 0:50670948e4d6 | 133 | { |
donsez | 0:50670948e4d6 | 134 | goLeftMotor(CW, MAXSPEED); |
donsez | 0:50670948e4d6 | 135 | goRightMotor(CCW, MAXSPEED); |
donsez | 0:50670948e4d6 | 136 | checkShield(); |
donsez | 0:50670948e4d6 | 137 | wait_ms(5000); |
donsez | 0:50670948e4d6 | 138 | |
donsez | 0:50670948e4d6 | 139 | stopLeftMotor(); |
donsez | 0:50670948e4d6 | 140 | stopRightMotor(); |
donsez | 0:50670948e4d6 | 141 | checkShield(); |
donsez | 0:50670948e4d6 | 142 | wait_ms(2000); |
donsez | 0:50670948e4d6 | 143 | |
donsez | 0:50670948e4d6 | 144 | goLeftMotor(CCW, MAXSPEED); |
donsez | 0:50670948e4d6 | 145 | goRightMotor(CW, MAXSPEED); |
donsez | 0:50670948e4d6 | 146 | checkShield(); |
donsez | 0:50670948e4d6 | 147 | wait_ms(5000); |
donsez | 0:50670948e4d6 | 148 | |
donsez | 0:50670948e4d6 | 149 | stopLeftMotor(); |
donsez | 0:50670948e4d6 | 150 | stopRightMotor(); |
donsez | 0:50670948e4d6 | 151 | checkShield(); |
donsez | 0:50670948e4d6 | 152 | wait_ms(2000); |
donsez | 0:50670948e4d6 | 153 | } |
donsez | 0:50670948e4d6 | 154 | |
donsez | 0:50670948e4d6 | 155 | int main() { |
donsez | 0:50670948e4d6 | 156 | setup(); |
donsez | 0:50670948e4d6 | 157 | while(1) { |
donsez | 0:50670948e4d6 | 158 | loop(); |
donsez | 0:50670948e4d6 | 159 | } |
donsez | 0:50670948e4d6 | 160 | } |
donsez | 0:50670948e4d6 | 161 |