Team DIANA
Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
Diff: main.cpp
- Revision:
- 1:97a0f449f19d
- Parent:
- 0:839902368a34
- Child:
- 2:fc8d58f9f5ce
diff -r 839902368a34 -r 97a0f449f19d main.cpp
--- a/main.cpp Thu Apr 11 16:15:01 2019 +0000
+++ b/main.cpp Fri May 03 15:07:35 2019 +0000
@@ -1,50 +1,81 @@
#include "mbed.h"
-#include "L6206_def.h"
+#include "L6206.h"
#include "BDCMotor.h"
-#include "QEI.h"
#include <math.h>
+Thread thread;
+
+
+void encoder() {
+ //Read the position by the encoder and save it in pos_encoder
+ wait(0.1);
+}
+void command() {
+ //Read the command (at the moment considered by me a normalized velocity)
+ wait(0.2);
+}
int main (int argc,char **argv) {
- //Prendo il comando -> fisso un punto da raggiungere (IK) (Automatico) -> In tot step voglio tale duty cicle
+ //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
+ //Common variables
+ float k = 0;
+ float v_max = 15; //mm/s
+ float pos_encoder = 0; //Posizione misurata dall'encoder
+ float k1 = 0;
+ float startPos_encoder = 0;
+ float frequency; //sampling frequency of the encoder
+ int Auto = 0;
+
+ //Auto variables
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 acceleration_range = 15; //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
-
+ float v_request = 0;
+ float PWM_dutyReduct = 0; //Variables used in case of triangular control
+ float acc_rangeReduct = 0;
+ float Contr_reference = 0; //Is the starting tracking error, needed to impose the right form of controller
+
+ //Manual variables
+ float v_normalized = 0; //Manual command that indicates the direction and the magnitude of the velocity
+ float v_actual = 0; //real velocity of the arm, computed through as incremental ratio
+ float delta_v=0; //Difference of velocities used as input for control
+ float a_max = 7.5; //maximum accelerazion mm/s^2
+ float time = 0;
+ float prev_pos = 0; //Previous potition to compute the velocity as an incremental ratio
+
+ //Initialization: to write the right pins
+ //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)
+
L6206 *motor;
- switch Auto {
-
- case 1 {/*The rover is in its automatic mode*/
+ /*Take initial pos encoder -> saved in starPos_encoder*/
+
+ pos_encoder = startPos_encoder;
+ switch (Auto) {
- //OTTENGO LA pozisione iniziale dell'encoder startPos_encoder
+ case 1: {/*The rover is in its automatic mode*/
+
+ //OTTENGO LA posizione iniziale dell'encoder startPos_encoder
//Ottengo il comando
- Contr_reference=finalDestination-startPos_encoder;
- track_error=Contr_reference;
+ 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
+ k1 = k;
+ k = (track_error/(finalDestination-acceleration_range))*100; //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)
+ k1 = k;
+ k = ((track_error)/acceleration_range)*100; //PWM_duty = 90*(1-k)
motor->set_speed(k1,k);
+
} else { //Can it be eliminated? (no command, no velocity variations)
/*v stays constant*/
}
@@ -53,39 +84,77 @@
} 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
+ 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;
+ PWM_dutyReduct = (Contr_reference/(2*acceleration_range))*100;
//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);
+ acc_rangeReduct = Contr_reference/2;
+ if (track_error>acc_rangeReduct) {
+ k1 = k;
+ k = (track_error/(finalDestination-acc_rangeReduct))*PWM_dutyReduct; //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)
+ k1 = k;
+ k = (track_error/acc_rangeReduct)*PWM_dutyReduct;
+ //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
+ 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;
+ default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
+ //normalized velocity
+ while(1) {
+
+ v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
+ delta_v = v_request-v_actual;
+ while (delta_v != 0) { //MUST add a condition about the maximum acceleration.
+ /*Choosing a maximum acceleration, preferebly linked to the condition of the syste (motor, structure, Pwm and so on).
+ Imposing an acceleration of 7.5 mm/s^2 to reach the v_max 2 secs are needed.*/
+ //Measure the new position
+ time = time+(1/frequency);
+
+ if (v_normalized > 0){
+ motor->run(1, BDCMotor::FWD);
+ k1 = k;
+ k = k+a_max/frequency*100/v_max;
+ motor->set_speed(k1,k);
+
+ } else if (v_normalized<0){
+ motor->run(1, BDCMotor::BWD);
+ k1 = k;
+ k = k+a_max/frequency*100/v_max;
+ motor->set_speed(k1,k);
+
+ } else{
+ k1 = k;
+ k = k-a_max/frequency*100/v_max;
+ motor->set_speed(k1,k);
+
+ }
+
+ prev_pos = pos_encoder;
+
+ v_actual = (pos_encoder-prev_pos)*frequency;
+ //In this computation is better to consider the velocity in the single period or the overall one?
+
+ delta_v = v_request-v_actual;
+ wait(0.1);
+ }
+
+ }
+ }
+ break;
+
+
+ }
}
-}
\ No newline at end of file