Diff: main.cpp

Revision:

1:dee6f3e0db9b

Parent:

0:3a3bfe92df7c

Child:

2:a5e166306da7

--- a/main.cpp	Fri Sep 19 15:26:02 2014 +0000
+++ b/main.cpp	Tue Apr 07 19:46:52 2015 +0000
@@ -1,19 +1,174 @@
+/******************************************************
+* Project CONNONBALL using NUCLEO STM32F401RE Year 2015 
+* @author Malek MAMMAR
+* @mail malek.mammar@gmail.com
+*******************************************************/
+
 #include "mbed.h"
 #include "Servo.h"
 
+/*    --> myservo.calibrate(0.00095, 90.0); // Calibrate the servo 
+      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 
+      a de 1.5-0.95 ms à 1.5+0.95 ms. 
+      La fonction position permet de positionner de facon relative a 1.5ms (90°) donc les valeurs permise sont de l'ordre -90 et +90
+      RMQ un simple position(0) permet de mettre le servo a l'angle 90°, position(-90°) = -90°, position(0°) = 0°, position(+90°) = +90°
+      La fonction write(percent) est identique mais en terme de pouventages avec write(0)=-90°, write(0.5%) = 90°, write(1)=+90°
+ */
+ 
+Servo steering(D5);
+Servo throttle(D6);
+Serial usb(USBTX, USBRX);
+//Serial usb(USBTX, USBRX, 1024, 512);
+DigitalOut led(LED1); 
+
+unsigned char steeringUSB = 95;
+unsigned char throttleUSB = 91;
+
+unsigned int time_data_check = 0;
+unsigned int time_data_check_last = 0;
+#define DEBUG 0
+#define signal 255
+#define throttle_stop_ms 1.5
+#define steering_right_deg 0
+
+Timer timer;
+
+
+void setup ();
+void loop();
+void loopTest(); //A test for understanding how the servos work
+void emergencyCall();
+void releaseSignal(); //Waits for the pc signal value
+void writeThrottle(float value);
+void writeSteering(float value);
+    
 int main()
 {
-    Servo myservo(D3);                // Create the servo object
-    AnalogIn knob(A0);                // Create the analog input object
-    
-    float val;
+    setup();
     
-    myservo.calibrate(0.00095, 90.0); // Calibrate the servo
+    while(1)
+        loop();
+}
+
+
+void setup () {
+    usb.baud(115200);
+    steering.calibrate(0.0005, 90.0); //!\\ Calibrate the servo  : essayer 45.0° a la place de 90.0°   
+    led = false;
+    releaseSignal(); //waits for the release signal from the pc
+    timer.start();
+    time_data_check = timer.read_ms();
+}
+
+void loop() {
+
+        if (usb.readable()) {
+            if (DEBUG)
+                led = true; 
+            steeringUSB = usb.getc();
+            //writeSteering(steeringUSB);
+            steering.position(steeringUSB - 95);
+            //usb.putc(steeringUSB);
+        //    time_data_check_last = time_data_check;
+            //time_data_check = timer.read_ms();
+            if (DEBUG) {
+        //        wait_ms(2000);
+                led = false; 
+            }
+        //}
+        //if (usb.readable()) {
+            if (DEBUG)
+                led = true; 
+            throttleUSB = usb.getc();
+            writeThrottle(throttleUSB);
+            //usb.putc(throttleUSB);
+            
+            time_data_check_last = time_data_check;
+            time_data_check = timer.read_ms();
+            
+            
+            
+            if (DEBUG) {
+          //      wait_ms(2000);
+                led = false; 
+            }
+        }
         
-    while(1)
-    {
-        val = knob.read();            // Reads the value of the potentiometer (value between 0 and 1) 
-        myservo.write(val);           // Sets the servo position according to the scaled value  (0-1)
-        wait_ms(15);                  // Waits for the servo to get there 
+        //steering.position(steeringUSB - 95);
+        //writeThrottle(throttleUSB);
+        //emergencyCall();
+        if(DEBUG){
+            //wait_ms(2000);
+        }
+        wait_ms(50);
+}
+
+void emergencyCall() {
+    int deltaTime = (DEBUG)? 10000 : 2000;
+    if (time_data_check_last - time_data_check > deltaTime) {
+        led = true;
+        while (1) {
+            throttleUSB = 91;
+            //steeringUSB = 95;
+            writeThrottle(throttleUSB);
+            //steering.write_ms(steering_right_ms);
+            //throttle.write_ms(steering_right_deg);
+            if (usb.readable() && usb.getc()==255) { //Release signal
+                led = false;
+                wait_ms(1000);
+                return;
+            }
+        }
     }
 }
+
+void writeSteering(float value) {
+    //Par rapport au code arduino : 40 à 96 (gauche) et de 96 à 140 (vers la droite)
+    //float _steeringRange = 140 - 40;
+    //write((steering/_steeringRange) + 0.5); //ratio de l'angle + 0.5 pour l'adapter a la fonction write ci dessus
+    steering.position(value - 96); //96 correspond a un angle droit des roues (code arduino), ce qui correspond ici a 0°
+}
+
+void writeThrottle(float value) {
+    //Par rapport au code arduino : point mort à 91 et pour la vitesse min = 40 et max = 180 
+    //on utilise les valeur 86, 88, 89
+    // Throttle pulsewidth : for a pulsewitdh >=1.5 ms or <0.75 ms the car stops
+    //float _throttleRange = 90-20; //? not sure about the range yet
+    if (value == 91) 
+        throttle.write_ms(1.5); 
+    else if (value == 90)
+        throttle.write_ms(1.49); 
+    else if ( value == 89 )
+        throttle.write_ms(1.48); 
+    else if ( value == 88 )
+        throttle.write_ms(1.47);
+    else if ( value == 86 )
+        throttle.write_ms(1.46);
+}
+
+void releaseSignal() {
+    while (1) {
+        if (usb.readable() && usb.getc()==255) { //Release signal
+            led = false;
+            return;
+        }
+    }
+}
+    
+
+void loopTest() {
+/*        //Test de la direction : les valeurs sont comprises entre 1.0 (gauche) et 2.0 (droite)
+        for(val=1.0; val<=2.0; val+=0.1){
+            steering.write_ms(val);
+            wait_ms(1000); 
+        }
+        
+        //Test de l'acceleration : les valeurs sont comprises entre 0.75 (rapide) a 1.49 (lentement). 
+        //Sachant qu'au-dessous de 1.0 (ie < 1.0) la vitesse semble être deja a son max.
+        //Remarque : l'appel a pwm.pulsewidth_ms() n'induit peut être pas le même comprtement --> se limiter a pwm.pulsewith(x/1000)
+        for(val=1.5; val<=1.0; val-=0.01){
+            throttle.write_ms(val);
+            wait_ms(1000); 
+        }
+*/      
+}
\ No newline at end of file