![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
Controller of the linear speed of the arm
Dependencies: QEI X_NUCLEO_IHM04A1
Diff: main.cpp
- Revision:
- 3:bbd927c5bfa9
- Parent:
- 2:fc8d58f9f5ce
- Child:
- 4:0b8f77ae7af0
diff -r fc8d58f9f5ce -r bbd927c5bfa9 main.cpp --- 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