Revision 3:bbd927c5bfa9, committed 2019-05-23

Comitter:

LuCordeschi

Date:

Thu May 23 07:00:53 2019 +0000

Parent:

2:fc8d58f9f5ce

Child:

4:0b8f77ae7af0

Commit message:

Speed arm controller

Changed in this revision

--- a/main.cpp	Fri May 03 15:19:43 2019 +0000
+++ b/main.cpp	Thu May 23 07:00:53 2019 +0000
@@ -2,17 +2,104 @@
 #include "L6206.h"
 #include "BDCMotor.h"
 #include <math.h>
+#include "QEI.h"
 
 Thread thread;
 
-void encoder() {
-    //Read the position by the encoder and save it in pos_encoder
-    wait(0.1);
+QEI encoder(PinName PB4, PinName PB5,PinName NC, int pulsesPerRev,QEI::Encoding X4_ENCODING); //Check the datasheet to know what is the pulses for revolution value
+//From the library it requires to pass NC in the index space in the case it is not present
+
+InterruptIn endstop (PinName PC1);
+DigitalOut led(LED1);
+InterruptIn button(PinName Button);
+
+
+L6206 *motor;
+
+L6206 L6206(PinName EN_A, PinName EN_B, PinName IN_1A, PinName IN_2A, PinName IN_1B, PinName IN_2B) ;
+
+void go_to_zero(float maxAcceleration,float maxVelocity, float EncoderResolution, int PulseperPeriod, int k, int k1, int t) {
+    motor->run(1, BDCMotor::BWD);
+        k=0;
+    while (t != 3) {
+        k1 = k;
+        k = k+maxAcceleration/EncoderResolution*100/maxVelocity;
+        motor->set_speed(k1,k);
+        PulseperPeriod = encoder.getPulses();
+
+        if (pos_encoder==0) {
+            t++;
+        } else {
+            t=0;
+        }
+    }
+}
+
+//Pin can: CANRX PB8 and CANTX PB9
+void canRxIsr() {
+    while(1) {
+
+    if(can1.read(messageIn))
+     
+        {
+            led1 = !led1;
+            printf("received\n\r");
+        }
+    }
 }
 void command() {
-    //Read the command (at the moment considered by me a normalized velocity)
-    wait(0.2);
-}    
+    
+    canRxIsr();
+    wait(0.5);
+}
+void sendMessage()
+{
+    int status = can.write(messageOut);
+   printf("Send status: %d\r\n", status);
+}
+
+void controller(float v_normalized, float v_max,float v_actual, float a_max, float resolution, int pos_encoder) {
+
+     float v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
+     float delta_v = v_request-v_actual;
+     float k=0;
+    
+    while (delta_v !=  0) {
+        /*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
+        
+        if (v_normalized > 0){
+            motor->run(1, BDCMotor::FWD);
+            float k1 = k;
+            k = k+  a_max/( resolution)*100/(v_max);
+            motor->set_speed(k1,k);
+                   
+        } else if (v_normalized<0){
+            motor->run(1, BDCMotor::BWD);
+            float k1 = k;
+            k = k+a_max/resolution*100/v_max;
+            motor->set_speed(k1,k);
+                     
+        } else{
+            float k1 = k;
+            k = k-a_max/resolution*100/v_max;
+            motor->set_speed(k1,k);
+                
+        }
+            pos_encoder = encoder.getPulses();
+
+            v_actual = (pos_encoder)*resolution;
+
+            delta_v = v_request-v_actual;
+
+            position = position + pos_encoder*/*Rad angle made (in average) by the encoder for each pulse*/;
+
+            wait(0.1);
+    }
+             
+}
+
 int main (int argc,char **argv) {
        
     //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
@@ -21,9 +108,9 @@
     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
+    float resolution; //sampling  = resolution of the encoder
     int Auto = 0;
+    int t=0;
 
     //Auto variables
     float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
@@ -41,17 +128,16 @@
     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
-    
+    float position=0; 
     //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;
+    gotozero(a_max,v_max,resolution, k, k1, t);
+
 
-    /*Take initial pos encoder -> saved in starPos_encoder*/
+    //Absolute position of the actuator (from can?)
     
-    pos_encoder = startPos_encoder;
-        switch (Auto) {
+            switch (Auto) {
 
             case 1: {/*The rover is in its automatic mode*/
 
@@ -75,9 +161,8 @@
                             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*/
-                        }
+                        } 
+
                         //Leggo la pos_encoder attuale
                             
                     } else { //Controllo triangolare
@@ -112,48 +197,19 @@
 
             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);
-                        }
-                          
-                    }
+      
+        
+                thread.start(controller);
+
+                thread.start(command);
+
+                while(1){
+
+                    sendMessage(); //position
+                            
                 }
-                break;
+                    
+            break;
             
-           
         }
-    }
+    }
\ No newline at end of file