![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
main.cpp
- Committer:
- LuCordeschi
- Date:
- 2019-05-03
- Revision:
- 1:97a0f449f19d
- Parent:
- 0:839902368a34
- Child:
- 2:fc8d58f9f5ce
File content as of revision 1:97a0f449f19d:
#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; } }