Team DIANA / Mbed OS speed_arm_controller

Controller of the linear speed of the arm

Dependencies:   QEI X_NUCLEO_IHM04A1

main.cpp@1:97a0f449f19d, 2019-05-03 (annotated)

Committer:
LuCordeschi
Date:
Fri May 03 15:07:35 2019 +0000
Revision:
1:97a0f449f19d
Parent:
0:839902368a34
Child:
2:fc8d58f9f5ce
Speed arm controller;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuCordeschi 0:839902368a34 1 #include "mbed.h"
LuCordeschi 1:97a0f449f19d 2 #include "L6206.h"
LuCordeschi 0:839902368a34 3 #include "BDCMotor.h"
LuCordeschi 0:839902368a34 4 #include <math.h>
LuCordeschi 0:839902368a34 5
LuCordeschi 1:97a0f449f19d 6 Thread thread;
LuCordeschi 1:97a0f449f19d 7
LuCordeschi 1:97a0f449f19d 8
LuCordeschi 1:97a0f449f19d 9 void encoder() {
LuCordeschi 1:97a0f449f19d 10 //Read the position by the encoder and save it in pos_encoder
LuCordeschi 1:97a0f449f19d 11 wait(0.1);
LuCordeschi 1:97a0f449f19d 12 }
LuCordeschi 1:97a0f449f19d 13 void command() {
LuCordeschi 1:97a0f449f19d 14 //Read the command (at the moment considered by me a normalized velocity)
LuCordeschi 1:97a0f449f19d 15 wait(0.2);
LuCordeschi 1:97a0f449f19d 16 }
LuCordeschi 0:839902368a34 17 int main (int argc,char **argv) {
LuCordeschi 0:839902368a34 18
LuCordeschi 1:97a0f449f19d 19 //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
LuCordeschi 1:97a0f449f19d 20 //Common variables
LuCordeschi 1:97a0f449f19d 21 float k = 0;
LuCordeschi 1:97a0f449f19d 22 float v_max = 15; //mm/s
LuCordeschi 1:97a0f449f19d 23 float pos_encoder = 0; //Posizione misurata dall'encoder
LuCordeschi 1:97a0f449f19d 24 float k1 = 0;
LuCordeschi 1:97a0f449f19d 25 float startPos_encoder = 0;
LuCordeschi 1:97a0f449f19d 26 float frequency; //sampling frequency of the encoder
LuCordeschi 1:97a0f449f19d 27 int Auto = 0;
LuCordeschi 1:97a0f449f19d 28
LuCordeschi 1:97a0f449f19d 29 //Auto variables
LuCordeschi 0:839902368a34 30 float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
LuCordeschi 1:97a0f449f19d 31 float acceleration_range = 15; //n° step in accelerazione/decelerazione. Deciso da me
LuCordeschi 1:97a0f449f19d 32 float track_error = 1;
LuCordeschi 0:839902368a34 33 float sens_error; //Error given by the amplitude of a step of the encoder
LuCordeschi 1:97a0f449f19d 34 float v_request = 0;
LuCordeschi 1:97a0f449f19d 35 float PWM_dutyReduct = 0; //Variables used in case of triangular control
LuCordeschi 1:97a0f449f19d 36 float acc_rangeReduct = 0;
LuCordeschi 1:97a0f449f19d 37 float Contr_reference = 0; //Is the starting tracking error, needed to impose the right form of controller
LuCordeschi 1:97a0f449f19d 38
LuCordeschi 1:97a0f449f19d 39 //Manual variables
LuCordeschi 1:97a0f449f19d 40 float v_normalized = 0; //Manual command that indicates the direction and the magnitude of the velocity
LuCordeschi 1:97a0f449f19d 41 float v_actual = 0; //real velocity of the arm, computed through as incremental ratio
LuCordeschi 1:97a0f449f19d 42 float delta_v=0; //Difference of velocities used as input for control
LuCordeschi 1:97a0f449f19d 43 float a_max = 7.5; //maximum accelerazion mm/s^2
LuCordeschi 1:97a0f449f19d 44 float time = 0;
LuCordeschi 1:97a0f449f19d 45 float prev_pos = 0; //Previous potition to compute the velocity as an incremental ratio
LuCordeschi 1:97a0f449f19d 46
LuCordeschi 1:97a0f449f19d 47 //Initialization: to write the right pins
LuCordeschi 1:97a0f449f19d 48 //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)
LuCordeschi 1:97a0f449f19d 49
LuCordeschi 0:839902368a34 50 L6206 *motor;
LuCordeschi 0:839902368a34 51
LuCordeschi 1:97a0f449f19d 52 /*Take initial pos encoder -> saved in starPos_encoder*/
LuCordeschi 1:97a0f449f19d 53
LuCordeschi 1:97a0f449f19d 54 pos_encoder = startPos_encoder;
LuCordeschi 1:97a0f449f19d 55 switch (Auto) {
LuCordeschi 0:839902368a34 56
LuCordeschi 1:97a0f449f19d 57 case 1: {/*The rover is in its automatic mode*/
LuCordeschi 1:97a0f449f19d 58
LuCordeschi 1:97a0f449f19d 59 //OTTENGO LA posizione iniziale dell'encoder startPos_encoder
LuCordeschi 0:839902368a34 60 //Ottengo il comando
LuCordeschi 1:97a0f449f19d 61 Contr_reference = finalDestination-startPos_encoder;
LuCordeschi 1:97a0f449f19d 62 track_error = Contr_reference;
LuCordeschi 0:839902368a34 63 while (track_error>sens_error) {
LuCordeschi 0:839902368a34 64
LuCordeschi 0:839902368a34 65 if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale
LuCordeschi 0:839902368a34 66
LuCordeschi 0:839902368a34 67 if (track_error>(finalDestination-acceleration_range)) {
LuCordeschi 1:97a0f449f19d 68 k1 = k;
LuCordeschi 1:97a0f449f19d 69 k = (track_error/(finalDestination-acceleration_range))*100; //V da imporre
LuCordeschi 0:839902368a34 70 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 71
LuCordeschi 0:839902368a34 72
LuCordeschi 0:839902368a34 73 } else if (track_error<acceleration_range) {
LuCordeschi 0:839902368a34 74
LuCordeschi 1:97a0f449f19d 75 k1 = k;
LuCordeschi 1:97a0f449f19d 76 k = ((track_error)/acceleration_range)*100; //PWM_duty = 90*(1-k)
LuCordeschi 0:839902368a34 77 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 78
LuCordeschi 0:839902368a34 79 } else { //Can it be eliminated? (no command, no velocity variations)
LuCordeschi 0:839902368a34 80 /*v stays constant*/
LuCordeschi 0:839902368a34 81 }
LuCordeschi 0:839902368a34 82 //Leggo la pos_encoder attuale
LuCordeschi 0:839902368a34 83
LuCordeschi 0:839902368a34 84 } else { //Controllo triangolare
LuCordeschi 0:839902368a34 85 /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore
LuCordeschi 0:839902368a34 86 In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde
LuCordeschi 1:97a0f449f19d 87 a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct = interval*90/40
LuCordeschi 0:839902368a34 88 */
LuCordeschi 0:839902368a34 89
LuCordeschi 1:97a0f449f19d 90 PWM_dutyReduct = (Contr_reference/(2*acceleration_range))*100;
LuCordeschi 0:839902368a34 91
LuCordeschi 0:839902368a34 92 //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range)
LuCordeschi 1:97a0f449f19d 93 acc_rangeReduct = Contr_reference/2;
LuCordeschi 1:97a0f449f19d 94 if (track_error>acc_rangeReduct) {
LuCordeschi 1:97a0f449f19d 95 k1 = k;
LuCordeschi 1:97a0f449f19d 96 k = (track_error/(finalDestination-acc_rangeReduct))*PWM_dutyReduct; //costante di proporzionalità
LuCordeschi 1:97a0f449f19d 97 //PWM_duty = k1*PWM_dutyReduct
LuCordeschi 1:97a0f449f19d 98 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 99
LuCordeschi 0:839902368a34 100 } else {
LuCordeschi 1:97a0f449f19d 101 k1 = k;
LuCordeschi 1:97a0f449f19d 102 k = (track_error/acc_rangeReduct)*PWM_dutyReduct;
LuCordeschi 1:97a0f449f19d 103 //PWM_duty = PWM_dutyReduct*(1-k)
LuCordeschi 0:839902368a34 104 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 105
LuCordeschi 0:839902368a34 106 }
LuCordeschi 0:839902368a34 107 //Leggo la pos_encoder attuale
LuCordeschi 0:839902368a34 108 }
LuCordeschi 1:97a0f449f19d 109 track_error = finalDestination-pos_encoder; //It is used as counter
LuCordeschi 0:839902368a34 110 }
LuCordeschi 0:839902368a34 111 }
LuCordeschi 0:839902368a34 112 break;
LuCordeschi 0:839902368a34 113
LuCordeschi 1:97a0f449f19d 114 default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
LuCordeschi 1:97a0f449f19d 115 //normalized velocity
LuCordeschi 1:97a0f449f19d 116 while(1) {
LuCordeschi 1:97a0f449f19d 117
LuCordeschi 1:97a0f449f19d 118 v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
LuCordeschi 1:97a0f449f19d 119 delta_v = v_request-v_actual;
LuCordeschi 1:97a0f449f19d 120 while (delta_v != 0) { //MUST add a condition about the maximum acceleration.
LuCordeschi 1:97a0f449f19d 121 /*Choosing a maximum acceleration, preferebly linked to the condition of the syste (motor, structure, Pwm and so on).
LuCordeschi 1:97a0f449f19d 122 Imposing an acceleration of 7.5 mm/s^2 to reach the v_max 2 secs are needed.*/
LuCordeschi 1:97a0f449f19d 123 //Measure the new position
LuCordeschi 1:97a0f449f19d 124 time = time+(1/frequency);
LuCordeschi 1:97a0f449f19d 125
LuCordeschi 1:97a0f449f19d 126 if (v_normalized > 0){
LuCordeschi 1:97a0f449f19d 127 motor->run(1, BDCMotor::FWD);
LuCordeschi 1:97a0f449f19d 128 k1 = k;
LuCordeschi 1:97a0f449f19d 129 k = k+a_max/frequency*100/v_max;
LuCordeschi 1:97a0f449f19d 130 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 131
LuCordeschi 1:97a0f449f19d 132 } else if (v_normalized<0){
LuCordeschi 1:97a0f449f19d 133 motor->run(1, BDCMotor::BWD);
LuCordeschi 1:97a0f449f19d 134 k1 = k;
LuCordeschi 1:97a0f449f19d 135 k = k+a_max/frequency*100/v_max;
LuCordeschi 1:97a0f449f19d 136 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 137
LuCordeschi 1:97a0f449f19d 138 } else{
LuCordeschi 1:97a0f449f19d 139 k1 = k;
LuCordeschi 1:97a0f449f19d 140 k = k-a_max/frequency*100/v_max;
LuCordeschi 1:97a0f449f19d 141 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 142
LuCordeschi 1:97a0f449f19d 143 }
LuCordeschi 1:97a0f449f19d 144
LuCordeschi 1:97a0f449f19d 145 prev_pos = pos_encoder;
LuCordeschi 1:97a0f449f19d 146
LuCordeschi 1:97a0f449f19d 147 v_actual = (pos_encoder-prev_pos)*frequency;
LuCordeschi 1:97a0f449f19d 148 //In this computation is better to consider the velocity in the single period or the overall one?
LuCordeschi 1:97a0f449f19d 149
LuCordeschi 1:97a0f449f19d 150 delta_v = v_request-v_actual;
LuCordeschi 1:97a0f449f19d 151 wait(0.1);
LuCordeschi 1:97a0f449f19d 152 }
LuCordeschi 1:97a0f449f19d 153
LuCordeschi 1:97a0f449f19d 154 }
LuCordeschi 1:97a0f449f19d 155 }
LuCordeschi 1:97a0f449f19d 156 break;
LuCordeschi 1:97a0f449f19d 157
LuCordeschi 1:97a0f449f19d 158
LuCordeschi 1:97a0f449f19d 159 }
LuCordeschi 0:839902368a34 160 }