Team DIANA / Mbed OS speed_arm_controller

Controller of the linear speed of the arm

Dependencies:   QEI X_NUCLEO_IHM04A1

main.cpp@4:0b8f77ae7af0, 2019-05-27 (annotated)

Committer:
LuCordeschi
Date:
Mon May 27 15:22:36 2019 +0000
Revision:
4:0b8f77ae7af0
Parent:
3:bbd927c5bfa9
Child:
5:ef270034f9a5
.

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