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@2:7f9d0d59c7f5, 2015-02-07 (annotated)

Committer:
Michel_Vivien
Date:
Sat Feb 07 19:38:19 2015 +0000
Revision:
2:7f9d0d59c7f5
Parent:
1:ec9cd1ae6f86 
Adaptation du code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
donsez 0:50670948e4d6 1 #include "mbed.h"
donsez 0:50670948e4d6 2 /*
Michel_Vivien 2:7f9d0d59c7f5 3 Example of Program for the MonsterMoto Shield on the ST Nucleo L152RE
Michel_Vivien 2:7f9d0d59c7f5 4 Code by : Vivien Michel
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 dirB.write(HIGH);
donsez 0:50670948e4d6 96 else
Michel_Vivien 1:ec9cd1ae6f86 97 dirB.write(LOW);
donsez 0:50670948e4d6 98
donsez 0:50670948e4d6 99 pwmLeftpin.write(percent);
donsez 0:50670948e4d6 100 }
donsez 0:50670948e4d6 101 }
donsez 0:50670948e4d6 102
donsez 0:50670948e4d6 103 void goRightMotor(uint8_t direct, float percent)
donsez 0:50670948e4d6 104 {
donsez 0:50670948e4d6 105 if (direct <=4)
donsez 0:50670948e4d6 106 {
donsez 0:50670948e4d6 107 if (direct <=1)
Michel_Vivien 1:ec9cd1ae6f86 108 dirA.write(HIGH);
Michel_Vivien 2:7f9d0d59c7f5 109 else
Michel_Vivien 1:ec9cd1ae6f86 110 dirA.write(LOW);
Michel_Vivien 2:7f9d0d59c7f5 111
donsez 0:50670948e4d6 112 pwmRightpin.write(percent);
donsez 0:50670948e4d6 113 }
donsez 0:50670948e4d6 114 }
donsez 0:50670948e4d6 115
donsez 0:50670948e4d6 116
donsez 0:50670948e4d6 117 void setup()
donsez 0:50670948e4d6 118 {
donsez 0:50670948e4d6 119 setupShield();
donsez 0:50670948e4d6 120 }
donsez 0:50670948e4d6 121
donsez 0:50670948e4d6 122 void loop()
donsez 0:50670948e4d6 123 {
donsez 0:50670948e4d6 124 goLeftMotor(CW, MAXSPEED);
donsez 0:50670948e4d6 125 goRightMotor(CCW, MAXSPEED);
Michel_Vivien 2:7f9d0d59c7f5 126 //checkShield();
donsez 0:50670948e4d6 127 wait_ms(5000);
donsez 0:50670948e4d6 128
donsez 0:50670948e4d6 129 stopLeftMotor();
donsez 0:50670948e4d6 130 stopRightMotor();
Michel_Vivien 2:7f9d0d59c7f5 131 //checkShield();
donsez 0:50670948e4d6 132 wait_ms(2000);
donsez 0:50670948e4d6 133
Michel_Vivien 2:7f9d0d59c7f5 134 goLeftMotor(CCW, 255.0f);
Michel_Vivien 2:7f9d0d59c7f5 135 goRightMotor(CW, 255.0f);
Michel_Vivien 2:7f9d0d59c7f5 136 //checkShield();
Michel_Vivien 2:7f9d0d59c7f5 137 wait_ms(3000);
donsez 0:50670948e4d6 138
donsez 0:50670948e4d6 139 stopLeftMotor();
donsez 0:50670948e4d6 140 stopRightMotor();
Michel_Vivien 2:7f9d0d59c7f5 141 //checkShield();
donsez 0:50670948e4d6 142 wait_ms(2000);
donsez 0:50670948e4d6 143 }
donsez 0:50670948e4d6 144
donsez 0:50670948e4d6 145 int main() {
donsez 0:50670948e4d6 146 setup();
donsez 0:50670948e4d6 147 while(1) {
donsez 0:50670948e4d6 148 loop();
donsez 0:50670948e4d6 149 }
donsez 0:50670948e4d6 150 }
donsez 0:50670948e4d6 151