Diff: main.cpp

Revision:

4:0b8f77ae7af0

Parent:

3:bbd927c5bfa9

Child:

5:ef270034f9a5

--- a/main.cpp	Thu May 23 07:00:53 2019 +0000
+++ b/main.cpp	Mon May 27 15:22:36 2019 +0000
@@ -4,28 +4,30 @@
 #include <math.h>
 #include "QEI.h"
 
+#define pulsesPerRev 45
 Thread thread;
 
-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
+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 (PinName PC1);
-DigitalOut led(LED1);
-InterruptIn button(PinName Button);
+InterruptIn endstop (PC_1);
+DigitalOut led1(LED1);
+InterruptIn button(USER_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) {
+void go_to_zero(float maxAcceleration,float maxVelocity, 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;
         motor->set_speed(k1,k);
-        PulseperPeriod = encoder.getPulses();
+        pos_encoder = encoder.getPulses();
 
         if (pos_encoder==0) {
             t++;
@@ -36,6 +38,15 @@
 }
 
 //Pin can: CANRX PB8 and CANTX PB9
+
+const PinName can1rxPins[] = {PB_8};
+const PinName can1txPins[] = {PB_9};
+
+CAN can1(can1rxPins[0], can1txPins[0]);
+
+CANMessage messageIn;
+CANMessage messageOut;
+
 void canRxIsr() {
     while(1) {
 
@@ -54,16 +65,18 @@
 }
 void sendMessage()
 {
-    int status = can.write(messageOut);
+    int status = can1.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) {
+void controller(float v_normalized, float v_max,float v_actual, float a_max, float resolution, int pos_encoder, float position, float prev_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;
-     float k=0;
+    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;
     
+    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.*/
@@ -93,8 +106,10 @@
 
             delta_v = v_request-v_actual;
 
-            position = position + pos_encoder*/*Rad angle made (in average) by the encoder for each pulse*/;
-
+            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);
     }
              
@@ -117,7 +132,6 @@
     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 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
@@ -125,14 +139,13 @@
     //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 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)
     
-    gotozero(a_max,v_max,resolution, k, k1, t);
+    go_to_zero(a_max, v_max, resolution ,pos_encoder , k, k1, t);
 
 
     //Absolute position of the actuator (from can?)
@@ -143,7 +156,7 @@
 
                 //OTTENGO LA posizione iniziale dell'encoder startPos_encoder
                 //Ottengo il comando 
-                Contr_reference = finalDestination-startPos_encoder;
+                Contr_reference = finalDestination-position;
                 track_error = Contr_reference;
                 while (track_error>sens_error) {
                     
@@ -197,9 +210,8 @@
 
             default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
                 //normalized velocity
-      
-        
-                thread.start(controller);
+                
+                controller(v_normalized, v_max, v_actual, a_max, resolution, pos_encoder, position, prev_position);
 
                 thread.start(command);
 
@@ -211,5 +223,6 @@
                     
             break;
             
+            }
         }
     }
\ No newline at end of file