main.cpp

00001 /******************************************************
00002 * Project CONNONBALL using NUCLEO STM32F401RE Year 2015 
00003 * @author Malek MAMMAR
00004 * @mail malek.mammar@gmail.com
00005 *******************************************************/
00006 
00007 #include "mbed.h"
00008 #include "Servo.h"
00009 #include "SerialDriver.h"
00010 
00011 /*    --> myservo.calibrate(0.00095, 90.0); // Calibrate the servo 
00012       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 
00013       a de 1.5-0.95 ms à 1.5+0.95 ms. 
00014       La fonction position permet de positionner de facon relative a 1.5ms (90°) donc les valeurs permise sont de l'ordre -90 et +90
00015       RMQ un simple position(0) permet de mettre le servo a l'angle 90°, position(-90°) = -90°, position(0°) = 0°, position(+90°) = +90°
00016       La fonction write(percent) est identique mais en terme de pouventages avec write(0)=-90°, write(0.5%) = 90°, write(1)=+90°
00017  */
00018  
00019 Servo steering(D5);
00020 Servo throttle(D6);
00021 //Serial usb(USBTX, USBRX);
00022 SerialDriver usb(USBTX, USBRX, 1024, 512);
00023 DigitalOut led(LED1); 
00024 
00025 unsigned char steeringUSB = 95;
00026 unsigned char throttleUSB = 91;
00027 
00028 unsigned int time_data_check = 0;
00029 unsigned int time_data_check_last = 0;
00030 #define DEBUG 1
00031 #define signal 255
00032 #define throttle_stop_ms 1.5
00033 #define steering_right_deg 0
00034 
00035 Timer timer;
00036 
00037 
00038 void setup ();
00039 void loop();
00040 void loopTest(); //A test for understanding how the servos work
00041 void emergencyCall();
00042 void releaseSignal(); //Waits for the pc signal value
00043 void writeThrottle(float value);
00044 void writeSteering(float value);
00045     
00046 int main()
00047 {
00048     setup();
00049     
00050     while(1)
00051         loop();
00052 }
00053 
00054 
00055 void setup () {
00056     usb.baud(115200);
00057     steering.calibrate(0.0005, 90.0); //!\\ Calibrate the servo  : essayer 45.0° a la place de 90.0°   
00058     led = false;
00059     releaseSignal(); //waits for the release signal from the pc
00060     timer.start();
00061     time_data_check = timer.read_ms();
00062 }
00063 
00064 void loop() {
00065 
00066         if (usb.readable()) {
00067             if (DEBUG)
00068                 led = true; 
00069             steeringUSB = usb.getc();
00070             //writeSteering(steeringUSB);
00071             steering.position(steeringUSB - 95);
00072             //usb.putc(steeringUSB); 
00073             //time_data_check_last = time_data_check;
00074             //time_data_check = timer.read_ms();
00075             if (DEBUG) {
00076                 //wait_ms(2000);
00077             //    led = false; 
00078             }
00079         //}
00080         //if (usb.readable()) {
00081             if (DEBUG)
00082                 led = true; 
00083             throttleUSB = usb.getc();
00084             writeThrottle(throttleUSB);
00085              
00086             //usb.putc(throttleUSB);
00087   
00088             time_data_check_last = time_data_check;
00089             time_data_check = timer.read_ms();
00090             
00091             if (DEBUG) {
00092           //      wait_ms(2000);
00093                 led = false; 
00094             }
00095         }
00096         
00097         //steering.position(steeringUSB - 95);
00098         //writeThrottle(throttleUSB);
00099         //emergencyCall();
00100         if(DEBUG){
00101             //wait_ms(2000);
00102             
00103         }
00104         wait_ms(50);
00105 }
00106 
00107 void emergencyCall() {
00108     int deltaTime = 10000; //(DEBUG)? 10000 : 4000;
00109     if (time_data_check_last - time_data_check > deltaTime) {
00110         led = true;
00111         while (1) {
00112             throttleUSB = 91;
00113             //steeringUSB = 95;
00114             writeThrottle(throttleUSB);
00115             //steering.write_ms(steering_right_ms);
00116             //throttle.write_ms(steering_right_deg);
00117             if (usb.readable() && usb.getc()==255) { //Release signal
00118                 led = false;
00119                 wait_ms(1000);
00120                 time_data_check_last = timer.read_ms();
00121                 return;
00122             }
00123         }
00124     }
00125 }
00126 
00127 void writeSteering(float value) {
00128     //Par rapport au code arduino : 40 à 96 (gauche) et de 96 à 140 (vers la droite)
00129     //float _steeringRange = 140 - 40;
00130     //write((steering/_steeringRange) + 0.5); //ratio de l'angle + 0.5 pour l'adapter a la fonction write ci dessus
00131     steering.position(value - 96); //96 correspond a un angle droit des roues (code arduino), ce qui correspond ici a 0°
00132 }
00133 
00134 void writeThrottle(float value) {
00135     //Par rapport au code arduino : point mort à 91 et pour la vitesse min = 40 et max = 180 
00136     //on utilise les valeur 86, 88, 89
00137     // Throttle pulsewidth : for a pulsewitdh >=1.5 ms or <0.75 ms the car stops
00138     //float _throttleRange = 90-20; //? not sure about the range yet
00139     if (value == 91) 
00140         throttle.write_ms(1.5); 
00141     else if (value == 90)
00142         throttle.write_ms(1.49); 
00143     else if ( value == 89 )
00144         throttle.write_ms(1.48); 
00145     else if ( value == 88 )
00146         throttle.write_ms(1.47);
00147     else if ( value == 86 )
00148         throttle.write_ms(1.46);
00149 }
00150 
00151 void releaseSignal() {
00152     while (1) {
00153         if (usb.readable() && usb.getc()==255) { //Release signal
00154             led = false;
00155             return;
00156         }
00157     }
00158 }
00159     
00160 
00161 void loopTest() {
00162 /*        //Test de la direction : les valeurs sont comprises entre 1.0 (gauche) et 2.0 (droite)
00163         for(val=1.0; val<=2.0; val+=0.1){
00164             steering.write_ms(val);
00165             wait_ms(1000); 
00166         }
00167         
00168         //Test de l'acceleration : les valeurs sont comprises entre 0.75 (rapide) a 1.49 (lentement). 
00169         //Sachant qu'au-dessous de 1.0 (ie < 1.0) la vitesse semble être deja a son max.
00170         //Remarque : l'appel a pwm.pulsewidth_ms() n'induit peut être pas le même comprtement --> se limiter a pwm.pulsewith(x/1000)
00171         for(val=1.5; val<=1.0; val-=0.01){
00172             throttle.write_ms(val);
00173             wait_ms(1000); 
00174         }
00175 */      
00176 }