![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
Diff: main.cpp
- Revision:
- 0:839902368a34
- Child:
- 1:97a0f449f19d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 11 16:15:01 2019 +0000 @@ -0,0 +1,91 @@ +#include "mbed.h" +#include "L6206_def.h" +#include "BDCMotor.h" +#include "QEI.h" +#include <math.h> + +int main (int argc,char **argv) { + + //Prendo il comando -> fisso un punto da raggiungere (IK) (Automatico) -> In tot step voglio tale duty cicle + float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO + float pos_encoder; //Posizione misurata dall'encoder + float acceleration_range=40; //n° step in accelerazione/decelerazione. Deciso da me + float track_error=1; + float sens_error; //Error given by the amplitude of a step of the encoder + float k=0; + float Contr_reference=0; //Is the starting tracking error, needed to impose the right form of controller + float k1=0; + float startPos_encoder=0; + float PWM_dutyReduct=0; //Variables used in case of triangular control + float acc_rangeReduct=0; + int Auto=0; + //Initialization + + L6206 *motor; + + switch Auto { + + case 1 {/*The rover is in its automatic mode*/ + + //OTTENGO LA pozisione iniziale dell'encoder startPos_encoder + //Ottengo il comando + Contr_reference=finalDestination-startPos_encoder; + track_error=Contr_reference; + while (track_error>sens_error) { + + if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale + + if (track_error>(finalDestination-acceleration_range)) { + k1=k; + k=(track_error/(finalDestination-acceleration_range))*90; //V da imporre + motor->set_speed(k1,k); + + } else if (track_error<acceleration_range) { + + k1=k; + k=((track_error)/acceleration_range)*90; //PWM_duty=90*(1-k) + motor->set_speed(k1,k); + } else { //Can it be eliminated? (no command, no velocity variations) + /*v stays constant*/ + } + //Leggo la pos_encoder attuale + + } else { //Controllo triangolare + /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore + In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde + a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct=interval*90/40 + */ + + PWM_dutyReduct=(Contr_reference/(2*acceleration_range))*90; + + //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range) + acc_rangeReduct=Contr_reference/2; + if (track_error>acceleration_range) { + k1=k; + k=(track_error/(finalDestination-acceleration_range)); //costante di proporzionalità + //PWM_duty=k1*PWM_dutyReduct + motor->set_speed(k1,k); + } else { + k1=k + k=(track_error/acceleration_range); + //PWM_duty=PWM_dutyReduct*(1-k) + motor->set_speed(k1,k); + } + //Leggo la pos_encoder attuale + } + track_error=finalDestination-pos_encoder; //It is used as counter + } + } + break; + + case 0 { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a + + } + break; + + default { + /*Error message*/ + } + break; + } +} \ No newline at end of file