File content as of revision 2:a5e166306da7:

/******************************************************
* Project CONNONBALL using NUCLEO STM32F401RE Year 2015 
* @author Malek MAMMAR
* @mail malek.mammar@gmail.com
*******************************************************/

#include "mbed.h"
#include "Servo.h"
#include "SerialDriver.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);
SerialDriver 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 1
#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()
{
    setup();
    
    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; 
            }
        }
        
        //steering.position(steeringUSB - 95);
        //writeThrottle(throttleUSB);
        //emergencyCall();
        if(DEBUG){
            //wait_ms(2000);
            
        }
        wait_ms(50);
}

void emergencyCall() {
    int deltaTime = 10000; //(DEBUG)? 10000 : 4000;
    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);
                time_data_check_last = timer.read_ms();
                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); 
        }
*/      
}