Team DIANA / Mbed OS speed_arm_controller

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?

UserRevisionLine numberNew 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 }