File content as of revision 5:ef270034f9a5:

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

#define v_max 15 //[mm/s]
#define a_max 7.5f
#define pulsesPerRev 45
#define a 135.1 //Distanza foro attuatore lineare- parte superiorie del braccio [mm]
#define b 482.25 //distanza foro superiore braccio- foro inferiore attuatore [mm]
    
    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 position=0; 
    float prev_position=0;
    float linear_position=0;
    float prev_linear_position=0;
    float delta_v;
    
    float k = 0;
    float pos_encoder = 0; //Posizione misurata dall'encoder
    float k1 = 0;
    float resolution; //sampling  = resolution of the encoder
    int Auto = 0;
    int t=0;

Thread thread;
QEI encoder(PB_4, PB_5,NC, pulsesPerRev, QEI::X4_ENCODING); //Check the datasheet to know what is the pulses for revolution value
//From the library it requires to pass NC in the index space in the case it is not present

InterruptIn end0(PC_1);

DigitalOut led1(LED1);
InterruptIn button(USER_BUTTON);



L6206 *motor;

//L6206 L6206(PinName EN_A,PinName  EN_B,PinName IN_1A,PinName IN_2A,PinName IN_1B,PinName IN_2B) ;
//sdd::Cout

void end0_int_handler(float position, float k, float k1) //10 mm back
{
    while ((10-linear_position)>0) {
    motor->run(1, BDCMotor::BWD);                        

    k1 = k;
    k = ((10-linear_position)/5)*100;
    //PWM_duty = PWM_dutyReduct*(1-k)
    motor->set_speed(k1,k);                
        
    pos_encoder=encoder.getPulses();
    
    position = prev_position + pos_encoder;       
            
    linear_position= sqrt(a*a+b*b-2*a*b*cos(position));

    }

}

void go_to_zero( float EncoderResolution, int pos_encoder, int k, int k1, int t) {
    motor->run(1, BDCMotor::BWD);
        k=0;
    while (t != 3) {
        k1 = k;
        k = k+a_max/EncoderResolution*100/v_max;
        motor->set_speed(k1,k);
        pos_encoder = encoder.getPulses();

        if (pos_encoder==0) {
            t++;
        } else {
            t=0;
        }
    }
}

//Pin can: CANRX PB8 and CANTX PB9

const PinName can1rxPins[] = {PB_8};
const PinName can1txPins[] = {PB_9};

CAN can1(can1rxPins[0], can1txPins[0]);

CANMessage messageIn;
CANMessage messageOut;

void canRxIsr() {
    while(1) {

    if(can1.read(messageIn))
     
        {
            led1 = !led1;
            printf("received\n\r");
        }
    }
}
void command() {
    
    canRxIsr();
    wait(0.5);
}

void sendMessage() {
    int status = can1.write(messageOut);
    printf("Send status: %d\r\n", status);
}

void controller(float v_normalized, float v_actual, float resolution, int pos_encoder, float position) {
    
    
    float v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
    delta_v = v_request-v_actual;
    float k=0;

    while (delta_v !=  0) {
        /*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
        
        if (v_normalized > 0){
            motor->run(1, BDCMotor::FWD);
            float k1 = k;
            k = k+ a_max/( resolution)*100/(v_max);
            motor->set_speed(k1,k);
                   
        } else if (v_normalized<0){
            motor->run(1, BDCMotor::BWD);
            float k1 = k;
            k = k+a_max/resolution*100/v_max;
            motor->set_speed(k1,k);
                     
        } else{
            float k1 = k;
            k = k-a_max/resolution*100/v_max;
            motor->set_speed(k1,k);
                
        }
            pos_encoder = encoder.getPulses();



            position = prev_position + pos_encoder;
            prev_position = position;
            
            linear_position= sqrt(a*a+b*b-2*a*b*cos(position));

            v_actual = (prev_linear_position-linear_position)*resolution;

            delta_v = v_request-v_actual;
            
            prev_linear_position=linear_position;

            wait(0.1);
    }
             
}

void controller_ausiliar (){
    controller(v_normalized, v_actual, resolution, pos_encoder, position);
}

int main (int argc,char **argv) {
       
    //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
    //Common variables

    //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 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
    float sens_error;
    //Manual variables
 
    go_to_zero(resolution ,pos_encoder , k, k1, t);

    //end0.rise(&end0_int_handler);
    

    //Absolute position of the actuator (from can?)
    
            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-position;
                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);
                           
                        } 

                        //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
                
                thread.start(&controller_ausiliar);
                thread.start(command);

                while(1){

                    
                    sendMessage(); //position
                            
                }
                    
            break;
            
            }
        }
     }