Revision 5:ef270034f9a5, committed 2019-07-20

Comitter:

LuCordeschi

Date:

Sat Jul 20 09:43:57 2019 +0000

Parent:

4:0b8f77ae7af0

Commit message:

.

Changed in this revision

diff -r 0b8f77ae7af0 -r ef270034f9a5 main.cpp
--- a/main.cpp	Mon May 27 15:22:36 2019 +0000
+++ b/main.cpp	Sat Jul 20 09:43:57 2019 +0000
@@ -4,13 +4,33 @@
 #include <math.h>
 #include "QEI.h"
 
+#define v_max 15 //[mm/s]
+#define a_max 7.5f
 #define pulsesPerRev 45
+#define a 135.1 //Distanza foro attuatore lineare- parte superiorie del braccio [mm]
+#define b 482.25 //distanza foro superiore braccio- foro inferiore attuatore [mm]
+    
+    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 position=0; 
+    float prev_position=0;
+    float linear_position=0;
+    float prev_linear_position=0;
+    float delta_v;
+    
+    float k = 0;
+    float pos_encoder = 0; //Posizione misurata dall'encoder
+    float k1 = 0;
+    float resolution; //sampling  = resolution of the encoder
+    int Auto = 0;
+    int t=0;
+
 Thread thread;
-
 QEI encoder(PB_4, PB_5,NC, pulsesPerRev, QEI::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 (PC_1);
+InterruptIn end0(PC_1);
+
 DigitalOut led1(LED1);
 InterruptIn button(USER_BUTTON);
 
@@ -18,14 +38,35 @@
 
 L6206 *motor;
 
-L6206 L6206(PinName EN_A, PinName EN_B, PinName IN_1A, PinName IN_2A, PinName IN_1B, PinName IN_2B) ;
+//L6206 L6206(PinName EN_A,PinName  EN_B,PinName IN_1A,PinName IN_2A,PinName IN_1B,PinName IN_2B) ;
+//sdd::Cout
+
+void end0_int_handler(float position, float k, float k1) //10 mm back
+{
+    while ((10-linear_position)>0) {
+    motor->run(1, BDCMotor::BWD);                        
 
-void go_to_zero(float maxAcceleration,float maxVelocity, float EncoderResolution, int pos_encoder, int k, int k1, int t) {
+    k1 = k;
+    k = ((10-linear_position)/5)*100;
+    //PWM_duty = PWM_dutyReduct*(1-k)
+    motor->set_speed(k1,k);                
+        
+    pos_encoder=encoder.getPulses();
+    
+    position = prev_position + pos_encoder;       
+            
+    linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
+
+    }
+
+}
+
+void go_to_zero( float EncoderResolution, int pos_encoder, int k, int k1, int t) {
     motor->run(1, BDCMotor::BWD);
         k=0;
     while (t != 3) {
         k1 = k;
-        k = k+maxAcceleration/EncoderResolution*100/maxVelocity;
+        k = k+a_max/EncoderResolution*100/v_max;
         motor->set_speed(k1,k);
         pos_encoder = encoder.getPulses();
 
@@ -63,20 +104,19 @@
     canRxIsr();
     wait(0.5);
 }
-void sendMessage()
-{
+
+void sendMessage() {
     int status = can1.write(messageOut);
-   printf("Send status: %d\r\n", status);
+    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 position, float prev_position) {
-
+void controller(float v_normalized, float v_actual, float resolution, int pos_encoder, float position) {
+    
+    
     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;
+    delta_v = v_request-v_actual;
     float k=0;
-    
-    prev_position=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.*/
@@ -85,7 +125,7 @@
         if (v_normalized > 0){
             motor->run(1, BDCMotor::FWD);
             float k1 = k;
-            k = k+  a_max/( resolution)*100/(v_max);
+            k = k+ a_max/( resolution)*100/(v_max);
             motor->set_speed(k1,k);
                    
         } else if (v_normalized<0){
@@ -102,51 +142,47 @@
         }
             pos_encoder = encoder.getPulses();
 
-            v_actual = (pos_encoder)*resolution;
+
+
+            position = prev_position + pos_encoder;
+            prev_position = position;
+            
+            linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
+
+            v_actual = (prev_linear_position-linear_position)*resolution;
 
             delta_v = v_request-v_actual;
+            
+            prev_linear_position=linear_position;
 
-            position = prev_position + pos_encoder /* to multiply for Rad angle made (in average) by the encoder for each pulse*/;
-            
-            prev_position = position;
-            
             wait(0.1);
     }
              
 }
 
+void controller_ausiliar (){
+    controller(v_normalized, v_actual, resolution, pos_encoder, position);
+}
+
 int main (int argc,char **argv) {
        
     //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 resolution; //sampling  = resolution of the encoder
-    int Auto = 0;
-    int t=0;
 
     //Auto variables
     float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
     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 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
-    
+    float sens_error;
     //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 a_max = 7.5; //maximum accelerazion mm/s^2
-    float position=0; 
-    float prev_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)
+ 
+    go_to_zero(resolution ,pos_encoder , k, k1, t);
+
+    //end0.rise(&end0_int_handler);
     
-    go_to_zero(a_max, v_max, resolution ,pos_encoder , k, k1, t);
-
 
     //Absolute position of the actuator (from can?)
     
@@ -193,7 +229,7 @@
                             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/acc_rangeReduct)*PWM_dutyReduct;
@@ -211,12 +247,12 @@
             default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
                 //normalized velocity
                 
-                controller(v_normalized, v_max, v_actual, a_max, resolution, pos_encoder, position, prev_position);
-
+                thread.start(&controller_ausiliar);
                 thread.start(command);
 
                 while(1){
 
+                    
                     sendMessage(); //position
                             
                 }
@@ -225,4 +261,4 @@
             
             }
         }
-    }
\ No newline at end of file
+     }
\ No newline at end of file