Diff: main.cpp

Revision:

0:839902368a34

Child:

1:97a0f449f19d

diff -r 000000000000 -r 839902368a34 main.cpp
--- /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