Team DIANA
Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
Revision 0:839902368a34, committed 2019-04-11
- Comitter:
- LuCordeschi
- Date:
- Thu Apr 11 16:15:01 2019 +0000
- Child:
- 1:97a0f449f19d
- Commit message:
- Skeleton of the code: automatic mode set.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Apr 11 16:15:01 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_IHM04A1.lib Thu Apr 11 16:15:01 2019 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM04A1/#d16ad1d58ea1
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 11 16:15:01 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file