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@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?

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 */
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