![](/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@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?
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 | 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 | } |