Team DIANA / Mbed OS speed_arm_controller

Controller of the linear speed of the arm

Dependencies:   QEI X_NUCLEO_IHM04A1

main.cpp@5:ef270034f9a5, 2019-07-20 (annotated)

Committer:
LuCordeschi
Date:
Sat Jul 20 09:43:57 2019 +0000
Revision:
5:ef270034f9a5
Parent:
4:0b8f77ae7af0 
.

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 3:bbd927c5bfa9 5 #include "QEI.h"
LuCordeschi 0:839902368a34 6
LuCordeschi 5:ef270034f9a5 7 #define v_max 15 //[mm/s]
LuCordeschi 5:ef270034f9a5 8 #define a_max 7.5f
LuCordeschi 4:0b8f77ae7af0 9 #define pulsesPerRev 45
LuCordeschi 5:ef270034f9a5 10 #define a 135.1 //Distanza foro attuatore lineare- parte superiorie del braccio [mm]
LuCordeschi 5:ef270034f9a5 11 #define b 482.25 //distanza foro superiore braccio- foro inferiore attuatore [mm]
LuCordeschi 5:ef270034f9a5 12
LuCordeschi 5:ef270034f9a5 13 float v_normalized = 0; //Manual command that indicates the direction and the magnitude of the velocity
LuCordeschi 5:ef270034f9a5 14 float v_actual = 0; //real velocity of the arm, computed through as incremental ratio
LuCordeschi 5:ef270034f9a5 15 float position=0;
LuCordeschi 5:ef270034f9a5 16 float prev_position=0;
LuCordeschi 5:ef270034f9a5 17 float linear_position=0;
LuCordeschi 5:ef270034f9a5 18 float prev_linear_position=0;
LuCordeschi 5:ef270034f9a5 19 float delta_v;
LuCordeschi 5:ef270034f9a5 20
LuCordeschi 5:ef270034f9a5 21 float k = 0;
LuCordeschi 5:ef270034f9a5 22 float pos_encoder = 0; //Posizione misurata dall'encoder
LuCordeschi 5:ef270034f9a5 23 float k1 = 0;
LuCordeschi 5:ef270034f9a5 24 float resolution; //sampling = resolution of the encoder
LuCordeschi 5:ef270034f9a5 25 int Auto = 0;
LuCordeschi 5:ef270034f9a5 26 int t=0;
LuCordeschi 5:ef270034f9a5 27
LuCordeschi 1:97a0f449f19d 28 Thread thread;
LuCordeschi 4:0b8f77ae7af0 29 QEI encoder(PB_4, PB_5,NC, pulsesPerRev, QEI::X4_ENCODING); //Check the datasheet to know what is the pulses for revolution value
LuCordeschi 3:bbd927c5bfa9 30 //From the library it requires to pass NC in the index space in the case it is not present
LuCordeschi 3:bbd927c5bfa9 31
LuCordeschi 5:ef270034f9a5 32 InterruptIn end0(PC_1);
LuCordeschi 5:ef270034f9a5 33
LuCordeschi 4:0b8f77ae7af0 34 DigitalOut led1(LED1);
LuCordeschi 4:0b8f77ae7af0 35 InterruptIn button(USER_BUTTON);
LuCordeschi 4:0b8f77ae7af0 36
LuCordeschi 3:bbd927c5bfa9 37
LuCordeschi 3:bbd927c5bfa9 38
LuCordeschi 3:bbd927c5bfa9 39 L6206 *motor;
LuCordeschi 3:bbd927c5bfa9 40
LuCordeschi 5:ef270034f9a5 41 //L6206 L6206(PinName EN_A,PinName EN_B,PinName IN_1A,PinName IN_2A,PinName IN_1B,PinName IN_2B) ;
LuCordeschi 5:ef270034f9a5 42 //sdd::Cout
LuCordeschi 5:ef270034f9a5 43
LuCordeschi 5:ef270034f9a5 44 void end0_int_handler(float position, float k, float k1) //10 mm back
LuCordeschi 5:ef270034f9a5 45 {
LuCordeschi 5:ef270034f9a5 46 while ((10-linear_position)>0) {
LuCordeschi 5:ef270034f9a5 47 motor->run(1, BDCMotor::BWD);
LuCordeschi 3:bbd927c5bfa9 48
LuCordeschi 5:ef270034f9a5 49 k1 = k;
LuCordeschi 5:ef270034f9a5 50 k = ((10-linear_position)/5)*100;
LuCordeschi 5:ef270034f9a5 51 //PWM_duty = PWM_dutyReduct*(1-k)
LuCordeschi 5:ef270034f9a5 52 motor->set_speed(k1,k);
LuCordeschi 5:ef270034f9a5 53
LuCordeschi 5:ef270034f9a5 54 pos_encoder=encoder.getPulses();
LuCordeschi 5:ef270034f9a5 55
LuCordeschi 5:ef270034f9a5 56 position = prev_position + pos_encoder;
LuCordeschi 5:ef270034f9a5 57
LuCordeschi 5:ef270034f9a5 58 linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
LuCordeschi 5:ef270034f9a5 59
LuCordeschi 5:ef270034f9a5 60 }
LuCordeschi 5:ef270034f9a5 61
LuCordeschi 5:ef270034f9a5 62 }
LuCordeschi 5:ef270034f9a5 63
LuCordeschi 5:ef270034f9a5 64 void go_to_zero( float EncoderResolution, int pos_encoder, int k, int k1, int t) {
LuCordeschi 3:bbd927c5bfa9 65 motor->run(1, BDCMotor::BWD);
LuCordeschi 3:bbd927c5bfa9 66 k=0;
LuCordeschi 3:bbd927c5bfa9 67 while (t != 3) {
LuCordeschi 3:bbd927c5bfa9 68 k1 = k;
LuCordeschi 5:ef270034f9a5 69 k = k+a_max/EncoderResolution*100/v_max;
LuCordeschi 3:bbd927c5bfa9 70 motor->set_speed(k1,k);
LuCordeschi 4:0b8f77ae7af0 71 pos_encoder = encoder.getPulses();
LuCordeschi 3:bbd927c5bfa9 72
LuCordeschi 3:bbd927c5bfa9 73 if (pos_encoder==0) {
LuCordeschi 3:bbd927c5bfa9 74 t++;
LuCordeschi 3:bbd927c5bfa9 75 } else {
LuCordeschi 3:bbd927c5bfa9 76 t=0;
LuCordeschi 3:bbd927c5bfa9 77 }
LuCordeschi 3:bbd927c5bfa9 78 }
LuCordeschi 3:bbd927c5bfa9 79 }
LuCordeschi 3:bbd927c5bfa9 80
LuCordeschi 3:bbd927c5bfa9 81 //Pin can: CANRX PB8 and CANTX PB9
LuCordeschi 4:0b8f77ae7af0 82
LuCordeschi 4:0b8f77ae7af0 83 const PinName can1rxPins[] = {PB_8};
LuCordeschi 4:0b8f77ae7af0 84 const PinName can1txPins[] = {PB_9};
LuCordeschi 4:0b8f77ae7af0 85
LuCordeschi 4:0b8f77ae7af0 86 CAN can1(can1rxPins[0], can1txPins[0]);
LuCordeschi 4:0b8f77ae7af0 87
LuCordeschi 4:0b8f77ae7af0 88 CANMessage messageIn;
LuCordeschi 4:0b8f77ae7af0 89 CANMessage messageOut;
LuCordeschi 4:0b8f77ae7af0 90
LuCordeschi 3:bbd927c5bfa9 91 void canRxIsr() {
LuCordeschi 3:bbd927c5bfa9 92 while(1) {
LuCordeschi 3:bbd927c5bfa9 93
LuCordeschi 3:bbd927c5bfa9 94 if(can1.read(messageIn))
LuCordeschi 3:bbd927c5bfa9 95
LuCordeschi 3:bbd927c5bfa9 96 {
LuCordeschi 3:bbd927c5bfa9 97 led1 = !led1;
LuCordeschi 3:bbd927c5bfa9 98 printf("received\n\r");
LuCordeschi 3:bbd927c5bfa9 99 }
LuCordeschi 3:bbd927c5bfa9 100 }
LuCordeschi 1:97a0f449f19d 101 }
LuCordeschi 1:97a0f449f19d 102 void command() {
LuCordeschi 3:bbd927c5bfa9 103
LuCordeschi 3:bbd927c5bfa9 104 canRxIsr();
LuCordeschi 3:bbd927c5bfa9 105 wait(0.5);
LuCordeschi 3:bbd927c5bfa9 106 }
LuCordeschi 5:ef270034f9a5 107
LuCordeschi 5:ef270034f9a5 108 void sendMessage() {
LuCordeschi 4:0b8f77ae7af0 109 int status = can1.write(messageOut);
LuCordeschi 5:ef270034f9a5 110 printf("Send status: %d\r\n", status);
LuCordeschi 3:bbd927c5bfa9 111 }
LuCordeschi 3:bbd927c5bfa9 112
LuCordeschi 5:ef270034f9a5 113 void controller(float v_normalized, float v_actual, float resolution, int pos_encoder, float position) {
LuCordeschi 5:ef270034f9a5 114
LuCordeschi 5:ef270034f9a5 115
LuCordeschi 4:0b8f77ae7af0 116 float v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
LuCordeschi 5:ef270034f9a5 117 delta_v = v_request-v_actual;
LuCordeschi 4:0b8f77ae7af0 118 float k=0;
LuCordeschi 5:ef270034f9a5 119
LuCordeschi 3:bbd927c5bfa9 120 while (delta_v != 0) {
LuCordeschi 3:bbd927c5bfa9 121 /*Choosing a maximum acceleration, preferebly linked to the condition of the syste (motor, structure, Pwm and so on).
LuCordeschi 3:bbd927c5bfa9 122 Imposing an acceleration of 7.5 mm/s^2 to reach the v_max 2 secs are needed.*/
LuCordeschi 3:bbd927c5bfa9 123 //Measure the new position
LuCordeschi 3:bbd927c5bfa9 124
LuCordeschi 3:bbd927c5bfa9 125 if (v_normalized > 0){
LuCordeschi 3:bbd927c5bfa9 126 motor->run(1, BDCMotor::FWD);
LuCordeschi 3:bbd927c5bfa9 127 float k1 = k;
LuCordeschi 5:ef270034f9a5 128 k = k+ a_max/( resolution)*100/(v_max);
LuCordeschi 3:bbd927c5bfa9 129 motor->set_speed(k1,k);
LuCordeschi 3:bbd927c5bfa9 130
LuCordeschi 3:bbd927c5bfa9 131 } else if (v_normalized<0){
LuCordeschi 3:bbd927c5bfa9 132 motor->run(1, BDCMotor::BWD);
LuCordeschi 3:bbd927c5bfa9 133 float k1 = k;
LuCordeschi 3:bbd927c5bfa9 134 k = k+a_max/resolution*100/v_max;
LuCordeschi 3:bbd927c5bfa9 135 motor->set_speed(k1,k);
LuCordeschi 3:bbd927c5bfa9 136
LuCordeschi 3:bbd927c5bfa9 137 } else{
LuCordeschi 3:bbd927c5bfa9 138 float k1 = k;
LuCordeschi 3:bbd927c5bfa9 139 k = k-a_max/resolution*100/v_max;
LuCordeschi 3:bbd927c5bfa9 140 motor->set_speed(k1,k);
LuCordeschi 3:bbd927c5bfa9 141
LuCordeschi 3:bbd927c5bfa9 142 }
LuCordeschi 3:bbd927c5bfa9 143 pos_encoder = encoder.getPulses();
LuCordeschi 3:bbd927c5bfa9 144
LuCordeschi 5:ef270034f9a5 145
LuCordeschi 5:ef270034f9a5 146
LuCordeschi 5:ef270034f9a5 147 position = prev_position + pos_encoder;
LuCordeschi 5:ef270034f9a5 148 prev_position = position;
LuCordeschi 5:ef270034f9a5 149
LuCordeschi 5:ef270034f9a5 150 linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
LuCordeschi 5:ef270034f9a5 151
LuCordeschi 5:ef270034f9a5 152 v_actual = (prev_linear_position-linear_position)*resolution;
LuCordeschi 3:bbd927c5bfa9 153
LuCordeschi 3:bbd927c5bfa9 154 delta_v = v_request-v_actual;
LuCordeschi 5:ef270034f9a5 155
LuCordeschi 5:ef270034f9a5 156 prev_linear_position=linear_position;
LuCordeschi 3:bbd927c5bfa9 157
LuCordeschi 3:bbd927c5bfa9 158 wait(0.1);
LuCordeschi 3:bbd927c5bfa9 159 }
LuCordeschi 3:bbd927c5bfa9 160
LuCordeschi 3:bbd927c5bfa9 161 }
LuCordeschi 3:bbd927c5bfa9 162
LuCordeschi 5:ef270034f9a5 163 void controller_ausiliar (){
LuCordeschi 5:ef270034f9a5 164 controller(v_normalized, v_actual, resolution, pos_encoder, position);
LuCordeschi 5:ef270034f9a5 165 }
LuCordeschi 5:ef270034f9a5 166
LuCordeschi 0:839902368a34 167 int main (int argc,char **argv) {
LuCordeschi 0:839902368a34 168
LuCordeschi 1:97a0f449f19d 169 //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
LuCordeschi 1:97a0f449f19d 170 //Common variables
LuCordeschi 1:97a0f449f19d 171
LuCordeschi 1:97a0f449f19d 172 //Auto variables
LuCordeschi 0:839902368a34 173 float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
LuCordeschi 1:97a0f449f19d 174 float acceleration_range = 15; //n° step in accelerazione/decelerazione. Deciso da me
LuCordeschi 1:97a0f449f19d 175 float track_error = 1;
LuCordeschi 1:97a0f449f19d 176 float PWM_dutyReduct = 0; //Variables used in case of triangular control
LuCordeschi 1:97a0f449f19d 177 float acc_rangeReduct = 0;
LuCordeschi 1:97a0f449f19d 178 float Contr_reference = 0; //Is the starting tracking error, needed to impose the right form of controller
LuCordeschi 5:ef270034f9a5 179 float sens_error;
LuCordeschi 1:97a0f449f19d 180 //Manual variables
LuCordeschi 5:ef270034f9a5 181
LuCordeschi 5:ef270034f9a5 182 go_to_zero(resolution ,pos_encoder , k, k1, t);
LuCordeschi 5:ef270034f9a5 183
LuCordeschi 5:ef270034f9a5 184 //end0.rise(&end0_int_handler);
LuCordeschi 1:97a0f449f19d 185
LuCordeschi 0:839902368a34 186
LuCordeschi 3:bbd927c5bfa9 187 //Absolute position of the actuator (from can?)
LuCordeschi 1:97a0f449f19d 188
LuCordeschi 3:bbd927c5bfa9 189 switch (Auto) {
LuCordeschi 0:839902368a34 190
LuCordeschi 1:97a0f449f19d 191 case 1: {/*The rover is in its automatic mode*/
LuCordeschi 1:97a0f449f19d 192
LuCordeschi 1:97a0f449f19d 193 //OTTENGO LA posizione iniziale dell'encoder startPos_encoder
LuCordeschi 0:839902368a34 194 //Ottengo il comando
LuCordeschi 4:0b8f77ae7af0 195 Contr_reference = finalDestination-position;
LuCordeschi 1:97a0f449f19d 196 track_error = Contr_reference;
LuCordeschi 0:839902368a34 197 while (track_error>sens_error) {
LuCordeschi 0:839902368a34 198
LuCordeschi 0:839902368a34 199 if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale
LuCordeschi 0:839902368a34 200
LuCordeschi 0:839902368a34 201 if (track_error>(finalDestination-acceleration_range)) {
LuCordeschi 1:97a0f449f19d 202 k1 = k;
LuCordeschi 1:97a0f449f19d 203 k = (track_error/(finalDestination-acceleration_range))*100; //V da imporre
LuCordeschi 0:839902368a34 204 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 205
LuCordeschi 0:839902368a34 206
LuCordeschi 0:839902368a34 207 } else if (track_error<acceleration_range) {
LuCordeschi 0:839902368a34 208
LuCordeschi 1:97a0f449f19d 209 k1 = k;
LuCordeschi 1:97a0f449f19d 210 k = ((track_error)/acceleration_range)*100; //PWM_duty = 90*(1-k)
LuCordeschi 0:839902368a34 211 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 212
LuCordeschi 3:bbd927c5bfa9 213 }
LuCordeschi 3:bbd927c5bfa9 214
LuCordeschi 0:839902368a34 215 //Leggo la pos_encoder attuale
LuCordeschi 0:839902368a34 216
LuCordeschi 0:839902368a34 217 } else { //Controllo triangolare
LuCordeschi 0:839902368a34 218 /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore
LuCordeschi 0:839902368a34 219 In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde
LuCordeschi 1:97a0f449f19d 220 a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct = interval*90/40
LuCordeschi 0:839902368a34 221 */
LuCordeschi 0:839902368a34 222
LuCordeschi 1:97a0f449f19d 223 PWM_dutyReduct = (Contr_reference/(2*acceleration_range))*100;
LuCordeschi 0:839902368a34 224
LuCordeschi 0:839902368a34 225 //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range)
LuCordeschi 1:97a0f449f19d 226 acc_rangeReduct = Contr_reference/2;
LuCordeschi 1:97a0f449f19d 227 if (track_error>acc_rangeReduct) {
LuCordeschi 1:97a0f449f19d 228 k1 = k;
LuCordeschi 1:97a0f449f19d 229 k = (track_error/(finalDestination-acc_rangeReduct))*PWM_dutyReduct; //costante di proporzionalità
LuCordeschi 1:97a0f449f19d 230 //PWM_duty = k1*PWM_dutyReduct
LuCordeschi 1:97a0f449f19d 231 motor->set_speed(k1,k);
LuCordeschi 5:ef270034f9a5 232
LuCordeschi 0:839902368a34 233 } else {
LuCordeschi 1:97a0f449f19d 234 k1 = k;
LuCordeschi 1:97a0f449f19d 235 k = (track_error/acc_rangeReduct)*PWM_dutyReduct;
LuCordeschi 1:97a0f449f19d 236 //PWM_duty = PWM_dutyReduct*(1-k)
LuCordeschi 0:839902368a34 237 motor->set_speed(k1,k);
LuCordeschi 1:97a0f449f19d 238
LuCordeschi 0:839902368a34 239 }
LuCordeschi 0:839902368a34 240 //Leggo la pos_encoder attuale
LuCordeschi 0:839902368a34 241 }
LuCordeschi 1:97a0f449f19d 242 track_error = finalDestination-pos_encoder; //It is used as counter
LuCordeschi 0:839902368a34 243 }
LuCordeschi 0:839902368a34 244 }
LuCordeschi 0:839902368a34 245 break;
LuCordeschi 0:839902368a34 246
LuCordeschi 1:97a0f449f19d 247 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 248 //normalized velocity
LuCordeschi 4:0b8f77ae7af0 249
LuCordeschi 5:ef270034f9a5 250 thread.start(&controller_ausiliar);
LuCordeschi 3:bbd927c5bfa9 251 thread.start(command);
LuCordeschi 3:bbd927c5bfa9 252
LuCordeschi 3:bbd927c5bfa9 253 while(1){
LuCordeschi 3:bbd927c5bfa9 254
LuCordeschi 5:ef270034f9a5 255
LuCordeschi 3:bbd927c5bfa9 256 sendMessage(); //position
LuCordeschi 3:bbd927c5bfa9 257
LuCordeschi 1:97a0f449f19d 258 }
LuCordeschi 3:bbd927c5bfa9 259
LuCordeschi 3:bbd927c5bfa9 260 break;
LuCordeschi 1:97a0f449f19d 261
LuCordeschi 4:0b8f77ae7af0 262 }
LuCordeschi 1:97a0f449f19d 263 }
LuCordeschi 5:ef270034f9a5 264 }