Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
main.cpp
- Committer:
- LuCordeschi
- Date:
- 2019-07-20
- Revision:
- 5:ef270034f9a5
- Parent:
- 4:0b8f77ae7af0
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; } } }