File content as of revision 2:fc8d58f9f5ce:

#include "mbed.h"
#include "L6206.h"
#include "BDCMotor.h"
#include <math.h>

Thread thread;

void encoder() {
    //Read the position by the encoder and save it in pos_encoder
    wait(0.1);
}
void command() {
    //Read the command (at the moment considered by me a normalized velocity)
    wait(0.2);
}    
int main (int argc,char **argv) {
       
    //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
    //Common variables
    float k = 0;
    float v_max = 15; //mm/s
    float pos_encoder = 0; //Posizione misurata dall'encoder
    float k1 = 0;
    float startPos_encoder = 0;
    float frequency; //sampling frequency of the encoder
    int Auto = 0;

    //Auto variables
    float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
    float acceleration_range = 15; //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 v_request = 0;
    float PWM_dutyReduct = 0; //Variables used in case of triangular control
    float acc_rangeReduct = 0;
    float Contr_reference = 0; //Is the starting tracking error, needed to impose the right form  of controller
    
    //Manual variables
    float v_normalized = 0; //Manual command that indicates the direction and the magnitude of the velocity
    float v_actual = 0; //real velocity of the arm, computed through as incremental ratio
    float delta_v=0; //Difference of velocities used as input for control
    float a_max = 7.5; //maximum accelerazion mm/s^2
    float time = 0;
    float prev_pos = 0; //Previous potition to compute the velocity as an incremental ratio
    
    //Initialization: to write the right pins
    //L6206(PinName EN_flag_A, PinName EN_flag_B, PinName pwm_1A, PinName pwm_2A, PinName pwm_1B, PinName pwm_2B) : BDCMotor(), flag_A_irq(EN_flag_A), flag_B_irq(EN_flag_B), EN_flag_A(EN_flag_A), EN_flag_B(EN_flag_B), pwm_1A(pwm_1A), pwm_2A(pwm_2A), pwm_1B(pwm_1B), pwm_2B(pwm_2B)
    
    L6206 *motor;

    /*Take initial pos encoder -> saved in starPos_encoder*/
    
    pos_encoder = startPos_encoder;
        switch (Auto) {

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

                //OTTENGO LA posizione 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))*100; //V da imporre
                            motor->set_speed(k1,k);
                           

                                } else if (track_error<acceleration_range) {
                                
                            k1 = k;
                            k = ((track_error)/acceleration_range)*100; //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))*100;
                             
                        //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>acc_rangeReduct) {
                            k1 = k;
                            k = (track_error/(finalDestination-acc_rangeReduct))*PWM_dutyReduct; //costante di proporzionalità
                            //PWM_duty = k1*PWM_dutyReduct
                            motor->set_speed(k1,k);
                                        
                        } else {
                            k1 = k;
                            k = (track_error/acc_rangeReduct)*PWM_dutyReduct;
                            //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;

            default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
                //normalized velocity
                while(1) {
    
                    v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
                    delta_v = v_request-v_actual;
                    while (delta_v !=  0) { //MUST add a condition about the maximum acceleration.
                        /*Choosing a maximum acceleration, preferebly linked to the condition of the syste (motor, structure, Pwm and so on).
                        Imposing an acceleration of 7.5 mm/s^2 to reach the v_max 2  secs are needed.*/
                        //Measure the new position
                        time = time+(1/frequency);
    
                        if (v_normalized > 0){
                            motor->run(1, BDCMotor::FWD);
                            k1 = k;
                            k = k+a_max/frequency*100/v_max;
                            motor->set_speed(k1,k);
                   
                        } else if (v_normalized<0){
                            motor->run(1, BDCMotor::BWD);
                            k1 = k;
                            k = k+a_max/frequency*100/v_max;
                            motor->set_speed(k1,k);
                     
                        } else{
                            k1 = k;
                            k = k-a_max/frequency*100/v_max;
                            motor->set_speed(k1,k);
                
                        }
    
                        prev_pos = pos_encoder;
    
                        v_actual = (pos_encoder-prev_pos)*frequency;
                        //In this computation is better to consider the velocity in the single period or the overall one?
                        
                        delta_v = v_request-v_actual;
                        wait(0.1);
                        }
                          
                    }
                }
                break;
            
           
        }
    }