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