Malek Malek / Mbed 2 deprecated nucleo_cannonball

cannon ball

Dependencies:   SerialDriver Servo mbed-rtos mbed

Fork of Nucleo_ServoKnob by Jose Rios

main.cpp@4:14c987860285, 2015-04-12 (annotated)

Committer:
Malek0512
Date:
Sun Apr 12 13:29:21 2015 +0000
Revision:
4:14c987860285
Parent:
3:bc919a54f665 
Version soutenance

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Malek0512 1:dee6f3e0db9b 1 /******************************************************
Malek0512 1:dee6f3e0db9b 2 * Project CONNONBALL using NUCLEO STM32F401RE Year 2015
Malek0512 1:dee6f3e0db9b 3 * @author Malek MAMMAR
Malek0512 1:dee6f3e0db9b 4 * @mail malek.mammar@gmail.com
Malek0512 1:dee6f3e0db9b 5 *******************************************************/
Malek0512 1:dee6f3e0db9b 6
jose_23991 0:3a3bfe92df7c 7 #include "mbed.h"
jose_23991 0:3a3bfe92df7c 8 #include "Servo.h"
Malek0512 2:a5e166306da7 9 #include "SerialDriver.h"
jose_23991 0:3a3bfe92df7c 10
Malek0512 1:dee6f3e0db9b 11 /* --> myservo.calibrate(0.00095, 90.0); // Calibrate the servo
Malek0512 1:dee6f3e0db9b 12 Le 0.00095 est l'ecart max entre la valeur 1.5ms qui correspond a une angle de 90 degrés : Pour ce deplacer de 0 à 180 degré on set le range
Malek0512 1:dee6f3e0db9b 13 a de 1.5-0.95 ms à 1.5+0.95 ms.
Malek0512 1:dee6f3e0db9b 14 La fonction position permet de positionner de facon relative a 1.5ms (90°) donc les valeurs permise sont de l'ordre -90 et +90
Malek0512 1:dee6f3e0db9b 15 RMQ un simple position(0) permet de mettre le servo a l'angle 90°, position(-90°) = -90°, position(0°) = 0°, position(+90°) = +90°
Malek0512 1:dee6f3e0db9b 16 La fonction write(percent) est identique mais en terme de pouventages avec write(0)=-90°, write(0.5%) = 90°, write(1)=+90°
Malek0512 1:dee6f3e0db9b 17 */
Malek0512 1:dee6f3e0db9b 18
Malek0512 1:dee6f3e0db9b 19 Servo steering(D5);
Malek0512 1:dee6f3e0db9b 20 Servo throttle(D6);
Malek0512 2:a5e166306da7 21 //Serial usb(USBTX, USBRX);
Malek0512 2:a5e166306da7 22 SerialDriver usb(USBTX, USBRX, 1024, 512);
Malek0512 1:dee6f3e0db9b 23 DigitalOut led(LED1);
Malek0512 1:dee6f3e0db9b 24
Malek0512 1:dee6f3e0db9b 25 unsigned char steeringUSB = 95;
Malek0512 1:dee6f3e0db9b 26 unsigned char throttleUSB = 91;
Malek0512 1:dee6f3e0db9b 27
Malek0512 1:dee6f3e0db9b 28 unsigned int time_data_check = 0;
Malek0512 1:dee6f3e0db9b 29 unsigned int time_data_check_last = 0;
Malek0512 2:a5e166306da7 30 #define DEBUG 1
Malek0512 1:dee6f3e0db9b 31 #define signal 255
Malek0512 1:dee6f3e0db9b 32 #define throttle_stop_ms 1.5
Malek0512 1:dee6f3e0db9b 33 #define steering_right_deg 0
Malek0512 1:dee6f3e0db9b 34
Malek0512 1:dee6f3e0db9b 35 Timer timer;
Malek0512 1:dee6f3e0db9b 36
Malek0512 1:dee6f3e0db9b 37
Malek0512 1:dee6f3e0db9b 38 void setup ();
Malek0512 1:dee6f3e0db9b 39 void loop();
Malek0512 1:dee6f3e0db9b 40 void loopTest(); //A test for understanding how the servos work
Malek0512 1:dee6f3e0db9b 41 void emergencyCall();
Malek0512 1:dee6f3e0db9b 42 void releaseSignal(); //Waits for the pc signal value
Malek0512 1:dee6f3e0db9b 43 void writeThrottle(float value);
Malek0512 1:dee6f3e0db9b 44 void writeSteering(float value);
Malek0512 1:dee6f3e0db9b 45
jose_23991 0:3a3bfe92df7c 46 int main()
jose_23991 0:3a3bfe92df7c 47 {
Malek0512 1:dee6f3e0db9b 48 setup();
jose_23991 0:3a3bfe92df7c 49
Malek0512 1:dee6f3e0db9b 50 while(1)
Malek0512 1:dee6f3e0db9b 51 loop();
Malek0512 1:dee6f3e0db9b 52 }
Malek0512 1:dee6f3e0db9b 53
Malek0512 1:dee6f3e0db9b 54
Malek0512 1:dee6f3e0db9b 55 void setup () {
Malek0512 1:dee6f3e0db9b 56 usb.baud(115200);
Malek0512 1:dee6f3e0db9b 57 steering.calibrate(0.0005, 90.0); //!\\ Calibrate the servo : essayer 45.0° a la place de 90.0°
Malek0512 1:dee6f3e0db9b 58 led = false;
Malek0512 1:dee6f3e0db9b 59 releaseSignal(); //waits for the release signal from the pc
Malek0512 1:dee6f3e0db9b 60 timer.start();
Malek0512 1:dee6f3e0db9b 61 time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 62 }
Malek0512 1:dee6f3e0db9b 63
Malek0512 1:dee6f3e0db9b 64 void loop() {
Malek0512 1:dee6f3e0db9b 65
Malek0512 1:dee6f3e0db9b 66 if (usb.readable()) {
Malek0512 1:dee6f3e0db9b 67 if (DEBUG)
Malek0512 1:dee6f3e0db9b 68 led = true;
Malek0512 1:dee6f3e0db9b 69 steeringUSB = usb.getc();
Malek0512 1:dee6f3e0db9b 70 //writeSteering(steeringUSB);
Malek0512 1:dee6f3e0db9b 71 steering.position(steeringUSB - 95);
Malek0512 4:14c987860285 72 //usb.putc(steeringUSB);
Malek0512 2:a5e166306da7 73 //time_data_check_last = time_data_check;
Malek0512 1:dee6f3e0db9b 74 //time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 75 if (DEBUG) {
Malek0512 2:a5e166306da7 76 //wait_ms(2000);
Malek0512 3:bc919a54f665 77 // led = false;
Malek0512 1:dee6f3e0db9b 78 }
Malek0512 1:dee6f3e0db9b 79 //}
Malek0512 1:dee6f3e0db9b 80 //if (usb.readable()) {
Malek0512 1:dee6f3e0db9b 81 if (DEBUG)
Malek0512 1:dee6f3e0db9b 82 led = true;
Malek0512 1:dee6f3e0db9b 83 throttleUSB = usb.getc();
Malek0512 1:dee6f3e0db9b 84 writeThrottle(throttleUSB);
Malek0512 2:a5e166306da7 85
Malek0512 4:14c987860285 86 //usb.putc(throttleUSB);
Malek0512 2:a5e166306da7 87
Malek0512 1:dee6f3e0db9b 88 time_data_check_last = time_data_check;
Malek0512 1:dee6f3e0db9b 89 time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 90
Malek0512 1:dee6f3e0db9b 91 if (DEBUG) {
Malek0512 1:dee6f3e0db9b 92 // wait_ms(2000);
Malek0512 1:dee6f3e0db9b 93 led = false;
Malek0512 1:dee6f3e0db9b 94 }
Malek0512 1:dee6f3e0db9b 95 }
jose_23991 0:3a3bfe92df7c 96
Malek0512 1:dee6f3e0db9b 97 //steering.position(steeringUSB - 95);
Malek0512 1:dee6f3e0db9b 98 //writeThrottle(throttleUSB);
Malek0512 1:dee6f3e0db9b 99 //emergencyCall();
Malek0512 1:dee6f3e0db9b 100 if(DEBUG){
Malek0512 1:dee6f3e0db9b 101 //wait_ms(2000);
Malek0512 2:a5e166306da7 102
Malek0512 1:dee6f3e0db9b 103 }
Malek0512 1:dee6f3e0db9b 104 wait_ms(50);
Malek0512 1:dee6f3e0db9b 105 }
Malek0512 1:dee6f3e0db9b 106
Malek0512 1:dee6f3e0db9b 107 void emergencyCall() {
Malek0512 2:a5e166306da7 108 int deltaTime = 10000; //(DEBUG)? 10000 : 4000;
Malek0512 1:dee6f3e0db9b 109 if (time_data_check_last - time_data_check > deltaTime) {
Malek0512 1:dee6f3e0db9b 110 led = true;
Malek0512 1:dee6f3e0db9b 111 while (1) {
Malek0512 1:dee6f3e0db9b 112 throttleUSB = 91;
Malek0512 1:dee6f3e0db9b 113 //steeringUSB = 95;
Malek0512 1:dee6f3e0db9b 114 writeThrottle(throttleUSB);
Malek0512 1:dee6f3e0db9b 115 //steering.write_ms(steering_right_ms);
Malek0512 1:dee6f3e0db9b 116 //throttle.write_ms(steering_right_deg);
Malek0512 1:dee6f3e0db9b 117 if (usb.readable() && usb.getc()==255) { //Release signal
Malek0512 1:dee6f3e0db9b 118 led = false;
Malek0512 1:dee6f3e0db9b 119 wait_ms(1000);
Malek0512 2:a5e166306da7 120 time_data_check_last = timer.read_ms();
Malek0512 1:dee6f3e0db9b 121 return;
Malek0512 1:dee6f3e0db9b 122 }
Malek0512 1:dee6f3e0db9b 123 }
jose_23991 0:3a3bfe92df7c 124 }
jose_23991 0:3a3bfe92df7c 125 }
Malek0512 1:dee6f3e0db9b 126
Malek0512 1:dee6f3e0db9b 127 void writeSteering(float value) {
Malek0512 1:dee6f3e0db9b 128 //Par rapport au code arduino : 40 à 96 (gauche) et de 96 à 140 (vers la droite)
Malek0512 1:dee6f3e0db9b 129 //float _steeringRange = 140 - 40;
Malek0512 1:dee6f3e0db9b 130 //write((steering/_steeringRange) + 0.5); //ratio de l'angle + 0.5 pour l'adapter a la fonction write ci dessus
Malek0512 1:dee6f3e0db9b 131 steering.position(value - 96); //96 correspond a un angle droit des roues (code arduino), ce qui correspond ici a 0°
Malek0512 1:dee6f3e0db9b 132 }
Malek0512 1:dee6f3e0db9b 133
Malek0512 1:dee6f3e0db9b 134 void writeThrottle(float value) {
Malek0512 1:dee6f3e0db9b 135 //Par rapport au code arduino : point mort à 91 et pour la vitesse min = 40 et max = 180
Malek0512 1:dee6f3e0db9b 136 //on utilise les valeur 86, 88, 89
Malek0512 1:dee6f3e0db9b 137 // Throttle pulsewidth : for a pulsewitdh >=1.5 ms or <0.75 ms the car stops
Malek0512 1:dee6f3e0db9b 138 //float _throttleRange = 90-20; //? not sure about the range yet
Malek0512 1:dee6f3e0db9b 139 if (value == 91)
Malek0512 1:dee6f3e0db9b 140 throttle.write_ms(1.5);
Malek0512 1:dee6f3e0db9b 141 else if (value == 90)
Malek0512 1:dee6f3e0db9b 142 throttle.write_ms(1.49);
Malek0512 1:dee6f3e0db9b 143 else if ( value == 89 )
Malek0512 1:dee6f3e0db9b 144 throttle.write_ms(1.48);
Malek0512 1:dee6f3e0db9b 145 else if ( value == 88 )
Malek0512 1:dee6f3e0db9b 146 throttle.write_ms(1.47);
Malek0512 1:dee6f3e0db9b 147 else if ( value == 86 )
Malek0512 1:dee6f3e0db9b 148 throttle.write_ms(1.46);
Malek0512 1:dee6f3e0db9b 149 }
Malek0512 1:dee6f3e0db9b 150
Malek0512 1:dee6f3e0db9b 151 void releaseSignal() {
Malek0512 1:dee6f3e0db9b 152 while (1) {
Malek0512 1:dee6f3e0db9b 153 if (usb.readable() && usb.getc()==255) { //Release signal
Malek0512 1:dee6f3e0db9b 154 led = false;
Malek0512 1:dee6f3e0db9b 155 return;
Malek0512 1:dee6f3e0db9b 156 }
Malek0512 1:dee6f3e0db9b 157 }
Malek0512 1:dee6f3e0db9b 158 }
Malek0512 1:dee6f3e0db9b 159
Malek0512 1:dee6f3e0db9b 160
Malek0512 1:dee6f3e0db9b 161 void loopTest() {
Malek0512 1:dee6f3e0db9b 162 /* //Test de la direction : les valeurs sont comprises entre 1.0 (gauche) et 2.0 (droite)
Malek0512 1:dee6f3e0db9b 163 for(val=1.0; val<=2.0; val+=0.1){
Malek0512 1:dee6f3e0db9b 164 steering.write_ms(val);
Malek0512 1:dee6f3e0db9b 165 wait_ms(1000);
Malek0512 1:dee6f3e0db9b 166 }
Malek0512 1:dee6f3e0db9b 167
Malek0512 1:dee6f3e0db9b 168 //Test de l'acceleration : les valeurs sont comprises entre 0.75 (rapide) a 1.49 (lentement).
Malek0512 1:dee6f3e0db9b 169 //Sachant qu'au-dessous de 1.0 (ie < 1.0) la vitesse semble être deja a son max.
Malek0512 1:dee6f3e0db9b 170 //Remarque : l'appel a pwm.pulsewidth_ms() n'induit peut être pas le même comprtement --> se limiter a pwm.pulsewith(x/1000)
Malek0512 1:dee6f3e0db9b 171 for(val=1.5; val<=1.0; val-=0.01){
Malek0512 1:dee6f3e0db9b 172 throttle.write_ms(val);
Malek0512 1:dee6f3e0db9b 173 wait_ms(1000);
Malek0512 1:dee6f3e0db9b 174 }
Malek0512 1:dee6f3e0db9b 175 */
Malek0512 1:dee6f3e0db9b 176 }