File content as of revision 0:839902368a34:

#include "mbed.h"
#include "L6206_def.h"
#include "BDCMotor.h"
#include "QEI.h"
#include <math.h>

int main (int argc,char **argv) {
       
    //Prendo il comando -> fisso un punto da raggiungere (IK) (Automatico) -> In tot step voglio tale duty cicle
    float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
    float pos_encoder; //Posizione misurata dall'encoder
    float acceleration_range=40; //n° step in accelerazione/decelerazione. Deciso da me 
    float track_error=1;
    float sens_error; //Error given by the amplitude of a step of the encoder
    float k=0;
    float Contr_reference=0; //Is the starting tracking error, needed to impose the right form  of controller
    float k1=0;
    float startPos_encoder=0;
    float PWM_dutyReduct=0; //Variables used in case of triangular control
    float acc_rangeReduct=0;
    int Auto=0;
    //Initialization

    L6206 *motor;

        switch Auto {

            case 1 {/*The rover is in its automatic mode*/

                //OTTENGO LA pozisione iniziale dell'encoder startPos_encoder
                //Ottengo il comando 
                Contr_reference=finalDestination-startPos_encoder;
                track_error=Contr_reference;
                while (track_error>sens_error) {
                    
                    if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale 
                        
                        if (track_error>(finalDestination-acceleration_range)) {
                            k1=k;
                            k=(track_error/(finalDestination-acceleration_range))*90; //V da imporre
                            motor->set_speed(k1,k);

                                } else if (track_error<acceleration_range) {
                                
                            k1=k;
                            k=((track_error)/acceleration_range)*90; //PWM_duty=90*(1-k)
                            motor->set_speed(k1,k);
                        } else { //Can it be eliminated? (no command, no velocity variations)
                            /*v stays constant*/
                        }
                        //Leggo la pos_encoder attuale
                            
                    } else { //Controllo triangolare
                        /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore
                        In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde
                        a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct=interval*90/40 
                        */

                        PWM_dutyReduct=(Contr_reference/(2*acceleration_range))*90;
                             
                        //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range)
                        acc_rangeReduct=Contr_reference/2;
                        if (track_error>acceleration_range) {
                            k1=k;
                            k=(track_error/(finalDestination-acceleration_range)); //costante di proporzionalità
                            //PWM_duty=k1*PWM_dutyReduct
                                motor->set_speed(k1,k);               
                        } else {
                            k1=k
                            k=(track_error/acceleration_range);
                            //PWM_duty=PWM_dutyReduct*(1-k)
                            motor->set_speed(k1,k);
                        }
                        //Leggo la pos_encoder attuale
                    }
                track_error=finalDestination-pos_encoder; //It is used as counter
                }
            }
            break;

            case 0 { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a

            }
            break;

            default {
                /*Error message*/
            }
            break;
    }
}