Vivien Michel / Mbed 2 deprecated RobAir_2014_2015

Example of Program for the MonsterMoto Shield on the ST Nucleo L152RE

Dependencies:   mbed

Fork of NucleoF401_MonsterMotoShield by Didier Donsez

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?

UserRevisionLine numberNew 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