Malek Malek / Mbed 2 deprecated nucleo_cannonball

cannon ball

Dependencies:   SerialDriver Servo mbed-rtos mbed

Fork of Nucleo_ServoKnob by Jose Rios

main.cpp@1:dee6f3e0db9b, 2015-04-07 (annotated)

Committer:
Malek0512
Date:
Tue Apr 07 19:46:52 2015 +0000
Revision:
1:dee6f3e0db9b
Parent:
0:3a3bfe92df7c
Child:
2:a5e166306da7
commit du soir

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"
jose_23991 0:3a3bfe92df7c 9
Malek0512 1:dee6f3e0db9b 10 /* --> myservo.calibrate(0.00095, 90.0); // Calibrate the servo
Malek0512 1:dee6f3e0db9b 11 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 12 a de 1.5-0.95 ms à 1.5+0.95 ms.
Malek0512 1:dee6f3e0db9b 13 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 14 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 15 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 16 */
Malek0512 1:dee6f3e0db9b 17
Malek0512 1:dee6f3e0db9b 18 Servo steering(D5);
Malek0512 1:dee6f3e0db9b 19 Servo throttle(D6);
Malek0512 1:dee6f3e0db9b 20 Serial usb(USBTX, USBRX);
Malek0512 1:dee6f3e0db9b 21 //Serial usb(USBTX, USBRX, 1024, 512);
Malek0512 1:dee6f3e0db9b 22 DigitalOut led(LED1);
Malek0512 1:dee6f3e0db9b 23
Malek0512 1:dee6f3e0db9b 24 unsigned char steeringUSB = 95;
Malek0512 1:dee6f3e0db9b 25 unsigned char throttleUSB = 91;
Malek0512 1:dee6f3e0db9b 26
Malek0512 1:dee6f3e0db9b 27 unsigned int time_data_check = 0;
Malek0512 1:dee6f3e0db9b 28 unsigned int time_data_check_last = 0;
Malek0512 1:dee6f3e0db9b 29 #define DEBUG 0
Malek0512 1:dee6f3e0db9b 30 #define signal 255
Malek0512 1:dee6f3e0db9b 31 #define throttle_stop_ms 1.5
Malek0512 1:dee6f3e0db9b 32 #define steering_right_deg 0
Malek0512 1:dee6f3e0db9b 33
Malek0512 1:dee6f3e0db9b 34 Timer timer;
Malek0512 1:dee6f3e0db9b 35
Malek0512 1:dee6f3e0db9b 36
Malek0512 1:dee6f3e0db9b 37 void setup ();
Malek0512 1:dee6f3e0db9b 38 void loop();
Malek0512 1:dee6f3e0db9b 39 void loopTest(); //A test for understanding how the servos work
Malek0512 1:dee6f3e0db9b 40 void emergencyCall();
Malek0512 1:dee6f3e0db9b 41 void releaseSignal(); //Waits for the pc signal value
Malek0512 1:dee6f3e0db9b 42 void writeThrottle(float value);
Malek0512 1:dee6f3e0db9b 43 void writeSteering(float value);
Malek0512 1:dee6f3e0db9b 44
jose_23991 0:3a3bfe92df7c 45 int main()
jose_23991 0:3a3bfe92df7c 46 {
Malek0512 1:dee6f3e0db9b 47 setup();
jose_23991 0:3a3bfe92df7c 48
Malek0512 1:dee6f3e0db9b 49 while(1)
Malek0512 1:dee6f3e0db9b 50 loop();
Malek0512 1:dee6f3e0db9b 51 }
Malek0512 1:dee6f3e0db9b 52
Malek0512 1:dee6f3e0db9b 53
Malek0512 1:dee6f3e0db9b 54 void setup () {
Malek0512 1:dee6f3e0db9b 55 usb.baud(115200);
Malek0512 1:dee6f3e0db9b 56 steering.calibrate(0.0005, 90.0); //!\\ Calibrate the servo : essayer 45.0° a la place de 90.0°
Malek0512 1:dee6f3e0db9b 57 led = false;
Malek0512 1:dee6f3e0db9b 58 releaseSignal(); //waits for the release signal from the pc
Malek0512 1:dee6f3e0db9b 59 timer.start();
Malek0512 1:dee6f3e0db9b 60 time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 61 }
Malek0512 1:dee6f3e0db9b 62
Malek0512 1:dee6f3e0db9b 63 void loop() {
Malek0512 1:dee6f3e0db9b 64
Malek0512 1:dee6f3e0db9b 65 if (usb.readable()) {
Malek0512 1:dee6f3e0db9b 66 if (DEBUG)
Malek0512 1:dee6f3e0db9b 67 led = true;
Malek0512 1:dee6f3e0db9b 68 steeringUSB = usb.getc();
Malek0512 1:dee6f3e0db9b 69 //writeSteering(steeringUSB);
Malek0512 1:dee6f3e0db9b 70 steering.position(steeringUSB - 95);
Malek0512 1:dee6f3e0db9b 71 //usb.putc(steeringUSB);
Malek0512 1:dee6f3e0db9b 72 // time_data_check_last = time_data_check;
Malek0512 1:dee6f3e0db9b 73 //time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 74 if (DEBUG) {
Malek0512 1:dee6f3e0db9b 75 // wait_ms(2000);
Malek0512 1:dee6f3e0db9b 76 led = false;
Malek0512 1:dee6f3e0db9b 77 }
Malek0512 1:dee6f3e0db9b 78 //}
Malek0512 1:dee6f3e0db9b 79 //if (usb.readable()) {
Malek0512 1:dee6f3e0db9b 80 if (DEBUG)
Malek0512 1:dee6f3e0db9b 81 led = true;
Malek0512 1:dee6f3e0db9b 82 throttleUSB = usb.getc();
Malek0512 1:dee6f3e0db9b 83 writeThrottle(throttleUSB);
Malek0512 1:dee6f3e0db9b 84 //usb.putc(throttleUSB);
Malek0512 1:dee6f3e0db9b 85
Malek0512 1:dee6f3e0db9b 86 time_data_check_last = time_data_check;
Malek0512 1:dee6f3e0db9b 87 time_data_check = timer.read_ms();
Malek0512 1:dee6f3e0db9b 88
Malek0512 1:dee6f3e0db9b 89
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 1:dee6f3e0db9b 102 }
Malek0512 1:dee6f3e0db9b 103 wait_ms(50);
Malek0512 1:dee6f3e0db9b 104 }
Malek0512 1:dee6f3e0db9b 105
Malek0512 1:dee6f3e0db9b 106 void emergencyCall() {
Malek0512 1:dee6f3e0db9b 107 int deltaTime = (DEBUG)? 10000 : 2000;
Malek0512 1:dee6f3e0db9b 108 if (time_data_check_last - time_data_check > deltaTime) {
Malek0512 1:dee6f3e0db9b 109 led = true;
Malek0512 1:dee6f3e0db9b 110 while (1) {
Malek0512 1:dee6f3e0db9b 111 throttleUSB = 91;
Malek0512 1:dee6f3e0db9b 112 //steeringUSB = 95;
Malek0512 1:dee6f3e0db9b 113 writeThrottle(throttleUSB);
Malek0512 1:dee6f3e0db9b 114 //steering.write_ms(steering_right_ms);
Malek0512 1:dee6f3e0db9b 115 //throttle.write_ms(steering_right_deg);
Malek0512 1:dee6f3e0db9b 116 if (usb.readable() && usb.getc()==255) { //Release signal
Malek0512 1:dee6f3e0db9b 117 led = false;
Malek0512 1:dee6f3e0db9b 118 wait_ms(1000);
Malek0512 1:dee6f3e0db9b 119 return;
Malek0512 1:dee6f3e0db9b 120 }
Malek0512 1:dee6f3e0db9b 121 }
jose_23991 0:3a3bfe92df7c 122 }
jose_23991 0:3a3bfe92df7c 123 }
Malek0512 1:dee6f3e0db9b 124
Malek0512 1:dee6f3e0db9b 125 void writeSteering(float value) {
Malek0512 1:dee6f3e0db9b 126 //Par rapport au code arduino : 40 à 96 (gauche) et de 96 à 140 (vers la droite)
Malek0512 1:dee6f3e0db9b 127 //float _steeringRange = 140 - 40;
Malek0512 1:dee6f3e0db9b 128 //write((steering/_steeringRange) + 0.5); //ratio de l'angle + 0.5 pour l'adapter a la fonction write ci dessus
Malek0512 1:dee6f3e0db9b 129 steering.position(value - 96); //96 correspond a un angle droit des roues (code arduino), ce qui correspond ici a 0°
Malek0512 1:dee6f3e0db9b 130 }
Malek0512 1:dee6f3e0db9b 131
Malek0512 1:dee6f3e0db9b 132 void writeThrottle(float value) {
Malek0512 1:dee6f3e0db9b 133 //Par rapport au code arduino : point mort à 91 et pour la vitesse min = 40 et max = 180
Malek0512 1:dee6f3e0db9b 134 //on utilise les valeur 86, 88, 89
Malek0512 1:dee6f3e0db9b 135 // Throttle pulsewidth : for a pulsewitdh >=1.5 ms or <0.75 ms the car stops
Malek0512 1:dee6f3e0db9b 136 //float _throttleRange = 90-20; //? not sure about the range yet
Malek0512 1:dee6f3e0db9b 137 if (value == 91)
Malek0512 1:dee6f3e0db9b 138 throttle.write_ms(1.5);
Malek0512 1:dee6f3e0db9b 139 else if (value == 90)
Malek0512 1:dee6f3e0db9b 140 throttle.write_ms(1.49);
Malek0512 1:dee6f3e0db9b 141 else if ( value == 89 )
Malek0512 1:dee6f3e0db9b 142 throttle.write_ms(1.48);
Malek0512 1:dee6f3e0db9b 143 else if ( value == 88 )
Malek0512 1:dee6f3e0db9b 144 throttle.write_ms(1.47);
Malek0512 1:dee6f3e0db9b 145 else if ( value == 86 )
Malek0512 1:dee6f3e0db9b 146 throttle.write_ms(1.46);
Malek0512 1:dee6f3e0db9b 147 }
Malek0512 1:dee6f3e0db9b 148
Malek0512 1:dee6f3e0db9b 149 void releaseSignal() {
Malek0512 1:dee6f3e0db9b 150 while (1) {
Malek0512 1:dee6f3e0db9b 151 if (usb.readable() && usb.getc()==255) { //Release signal
Malek0512 1:dee6f3e0db9b 152 led = false;
Malek0512 1:dee6f3e0db9b 153 return;
Malek0512 1:dee6f3e0db9b 154 }
Malek0512 1:dee6f3e0db9b 155 }
Malek0512 1:dee6f3e0db9b 156 }
Malek0512 1:dee6f3e0db9b 157
Malek0512 1:dee6f3e0db9b 158
Malek0512 1:dee6f3e0db9b 159 void loopTest() {
Malek0512 1:dee6f3e0db9b 160 /* //Test de la direction : les valeurs sont comprises entre 1.0 (gauche) et 2.0 (droite)
Malek0512 1:dee6f3e0db9b 161 for(val=1.0; val<=2.0; val+=0.1){
Malek0512 1:dee6f3e0db9b 162 steering.write_ms(val);
Malek0512 1:dee6f3e0db9b 163 wait_ms(1000);
Malek0512 1:dee6f3e0db9b 164 }
Malek0512 1:dee6f3e0db9b 165
Malek0512 1:dee6f3e0db9b 166 //Test de l'acceleration : les valeurs sont comprises entre 0.75 (rapide) a 1.49 (lentement).
Malek0512 1:dee6f3e0db9b 167 //Sachant qu'au-dessous de 1.0 (ie < 1.0) la vitesse semble être deja a son max.
Malek0512 1:dee6f3e0db9b 168 //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 169 for(val=1.5; val<=1.0; val-=0.01){
Malek0512 1:dee6f3e0db9b 170 throttle.write_ms(val);
Malek0512 1:dee6f3e0db9b 171 wait_ms(1000);
Malek0512 1:dee6f3e0db9b 172 }
Malek0512 1:dee6f3e0db9b 173 */
Malek0512 1:dee6f3e0db9b 174 }