![](/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@0:839902368a34, 2019-04-11 (annotated)
- Committer:
- LuCordeschi
- Date:
- Thu Apr 11 16:15:01 2019 +0000
- Revision:
- 0:839902368a34
- Child:
- 1:97a0f449f19d
Skeleton of the code: automatic mode set.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LuCordeschi | 0:839902368a34 | 1 | #include "mbed.h" |
LuCordeschi | 0:839902368a34 | 2 | #include "L6206_def.h" |
LuCordeschi | 0:839902368a34 | 3 | #include "BDCMotor.h" |
LuCordeschi | 0:839902368a34 | 4 | #include "QEI.h" |
LuCordeschi | 0:839902368a34 | 5 | #include <math.h> |
LuCordeschi | 0:839902368a34 | 6 | |
LuCordeschi | 0:839902368a34 | 7 | int main (int argc,char **argv) { |
LuCordeschi | 0:839902368a34 | 8 | |
LuCordeschi | 0:839902368a34 | 9 | //Prendo il comando -> fisso un punto da raggiungere (IK) (Automatico) -> In tot step voglio tale duty cicle |
LuCordeschi | 0:839902368a34 | 10 | float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO |
LuCordeschi | 0:839902368a34 | 11 | float pos_encoder; //Posizione misurata dall'encoder |
LuCordeschi | 0:839902368a34 | 12 | float acceleration_range=40; //n° step in accelerazione/decelerazione. Deciso da me |
LuCordeschi | 0:839902368a34 | 13 | float track_error=1; |
LuCordeschi | 0:839902368a34 | 14 | float sens_error; //Error given by the amplitude of a step of the encoder |
LuCordeschi | 0:839902368a34 | 15 | float k=0; |
LuCordeschi | 0:839902368a34 | 16 | float Contr_reference=0; //Is the starting tracking error, needed to impose the right form of controller |
LuCordeschi | 0:839902368a34 | 17 | float k1=0; |
LuCordeschi | 0:839902368a34 | 18 | float startPos_encoder=0; |
LuCordeschi | 0:839902368a34 | 19 | float PWM_dutyReduct=0; //Variables used in case of triangular control |
LuCordeschi | 0:839902368a34 | 20 | float acc_rangeReduct=0; |
LuCordeschi | 0:839902368a34 | 21 | int Auto=0; |
LuCordeschi | 0:839902368a34 | 22 | //Initialization |
LuCordeschi | 0:839902368a34 | 23 | |
LuCordeschi | 0:839902368a34 | 24 | L6206 *motor; |
LuCordeschi | 0:839902368a34 | 25 | |
LuCordeschi | 0:839902368a34 | 26 | switch Auto { |
LuCordeschi | 0:839902368a34 | 27 | |
LuCordeschi | 0:839902368a34 | 28 | case 1 {/*The rover is in its automatic mode*/ |
LuCordeschi | 0:839902368a34 | 29 | |
LuCordeschi | 0:839902368a34 | 30 | //OTTENGO LA pozisione iniziale dell'encoder startPos_encoder |
LuCordeschi | 0:839902368a34 | 31 | //Ottengo il comando |
LuCordeschi | 0:839902368a34 | 32 | Contr_reference=finalDestination-startPos_encoder; |
LuCordeschi | 0:839902368a34 | 33 | track_error=Contr_reference; |
LuCordeschi | 0:839902368a34 | 34 | while (track_error>sens_error) { |
LuCordeschi | 0:839902368a34 | 35 | |
LuCordeschi | 0:839902368a34 | 36 | if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale |
LuCordeschi | 0:839902368a34 | 37 | |
LuCordeschi | 0:839902368a34 | 38 | if (track_error>(finalDestination-acceleration_range)) { |
LuCordeschi | 0:839902368a34 | 39 | k1=k; |
LuCordeschi | 0:839902368a34 | 40 | k=(track_error/(finalDestination-acceleration_range))*90; //V da imporre |
LuCordeschi | 0:839902368a34 | 41 | motor->set_speed(k1,k); |
LuCordeschi | 0:839902368a34 | 42 | |
LuCordeschi | 0:839902368a34 | 43 | } else if (track_error<acceleration_range) { |
LuCordeschi | 0:839902368a34 | 44 | |
LuCordeschi | 0:839902368a34 | 45 | k1=k; |
LuCordeschi | 0:839902368a34 | 46 | k=((track_error)/acceleration_range)*90; //PWM_duty=90*(1-k) |
LuCordeschi | 0:839902368a34 | 47 | motor->set_speed(k1,k); |
LuCordeschi | 0:839902368a34 | 48 | } else { //Can it be eliminated? (no command, no velocity variations) |
LuCordeschi | 0:839902368a34 | 49 | /*v stays constant*/ |
LuCordeschi | 0:839902368a34 | 50 | } |
LuCordeschi | 0:839902368a34 | 51 | //Leggo la pos_encoder attuale |
LuCordeschi | 0:839902368a34 | 52 | |
LuCordeschi | 0:839902368a34 | 53 | } else { //Controllo triangolare |
LuCordeschi | 0:839902368a34 | 54 | /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore |
LuCordeschi | 0:839902368a34 | 55 | In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde |
LuCordeschi | 0:839902368a34 | 56 | a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct=interval*90/40 |
LuCordeschi | 0:839902368a34 | 57 | */ |
LuCordeschi | 0:839902368a34 | 58 | |
LuCordeschi | 0:839902368a34 | 59 | PWM_dutyReduct=(Contr_reference/(2*acceleration_range))*90; |
LuCordeschi | 0:839902368a34 | 60 | |
LuCordeschi | 0:839902368a34 | 61 | //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range) |
LuCordeschi | 0:839902368a34 | 62 | acc_rangeReduct=Contr_reference/2; |
LuCordeschi | 0:839902368a34 | 63 | if (track_error>acceleration_range) { |
LuCordeschi | 0:839902368a34 | 64 | k1=k; |
LuCordeschi | 0:839902368a34 | 65 | k=(track_error/(finalDestination-acceleration_range)); //costante di proporzionalità |
LuCordeschi | 0:839902368a34 | 66 | //PWM_duty=k1*PWM_dutyReduct |
LuCordeschi | 0:839902368a34 | 67 | motor->set_speed(k1,k); |
LuCordeschi | 0:839902368a34 | 68 | } else { |
LuCordeschi | 0:839902368a34 | 69 | k1=k |
LuCordeschi | 0:839902368a34 | 70 | k=(track_error/acceleration_range); |
LuCordeschi | 0:839902368a34 | 71 | //PWM_duty=PWM_dutyReduct*(1-k) |
LuCordeschi | 0:839902368a34 | 72 | motor->set_speed(k1,k); |
LuCordeschi | 0:839902368a34 | 73 | } |
LuCordeschi | 0:839902368a34 | 74 | //Leggo la pos_encoder attuale |
LuCordeschi | 0:839902368a34 | 75 | } |
LuCordeschi | 0:839902368a34 | 76 | track_error=finalDestination-pos_encoder; //It is used as counter |
LuCordeschi | 0:839902368a34 | 77 | } |
LuCordeschi | 0:839902368a34 | 78 | } |
LuCordeschi | 0:839902368a34 | 79 | break; |
LuCordeschi | 0:839902368a34 | 80 | |
LuCordeschi | 0:839902368a34 | 81 | case 0 { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a |
LuCordeschi | 0:839902368a34 | 82 | |
LuCordeschi | 0:839902368a34 | 83 | } |
LuCordeschi | 0:839902368a34 | 84 | break; |
LuCordeschi | 0:839902368a34 | 85 | |
LuCordeschi | 0:839902368a34 | 86 | default { |
LuCordeschi | 0:839902368a34 | 87 | /*Error message*/ |
LuCordeschi | 0:839902368a34 | 88 | } |
LuCordeschi | 0:839902368a34 | 89 | break; |
LuCordeschi | 0:839902368a34 | 90 | } |
LuCordeschi | 0:839902368a34 | 91 | } |