Team DIANA / Mbed OS speed_arm_controller

Controller of the linear speed of the arm

Dependencies:   QEI X_NUCLEO_IHM04A1

main.cpp@2:fc8d58f9f5ce, 2019-05-03 (annotated)

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