Group 9 BioRobotics / Mbed 2 deprecated newRKI

RKI

Dependencies:   QEI mbed

main.cpp@8:bc4b89961dd5, 2018-11-01 (annotated)

Committer:
s1725696
Date:
Thu Nov 01 11:14:15 2018 +0000
Revision:
8:bc4b89961dd5
Parent:
7:29c93152d96a 
final version, implemented in big code so do not change here

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kweisbeek 0:7789750c3b36 1 #include "mbed.h"
kweisbeek 0:7789750c3b36 2 #include "QEI.h"
kweisbeek 0:7789750c3b36 3 #define SERIAL_BAUD 115200
kweisbeek 0:7789750c3b36 4
s1725696 8:bc4b89961dd5 5 /* IMPLEMENTED IN BIG FUNCTION, SO DO NOT CHANGE HERE BUT THERE!!!!!*/
s1725696 8:bc4b89961dd5 6
s1725696 8:bc4b89961dd5 7
kweisbeek 0:7789750c3b36 8 //Initial allocations
kweisbeek 0:7789750c3b36 9 Serial pc(USBTX,USBRX);
kweisbeek 0:7789750c3b36 10
kweisbeek 0:7789750c3b36 11 AnalogIn pot1(A1);
kweisbeek 0:7789750c3b36 12 AnalogIn pot2(A2);
kweisbeek 0:7789750c3b36 13
kweisbeek 3:a07e8f090946 14 DigitalOut dirpin(D7); //Motor 1 (Rotatie)
kweisbeek 2:d41aa6d0e3cc 15 PwmOut pwmpin(D6);
kweisbeek 3:a07e8f090946 16
kweisbeek 3:a07e8f090946 17 DigitalOut dirpin2(D4); //Motor 2 (Translatie)
kweisbeek 2:d41aa6d0e3cc 18 PwmOut pwmpin2(D5);
kweisbeek 3:a07e8f090946 19
kweisbeek 3:a07e8f090946 20 QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING); //Encoder motor 1
kweisbeek 3:a07e8f090946 21 QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING); // Encoder motor 2
kweisbeek 0:7789750c3b36 22
s1574396 5:0bbb52e29790 23 const double Ts = 0.001;// sample frequency
s1574396 5:0bbb52e29790 24
s1574396 4:d568f61be1d1 25 //constants motor
kweisbeek 3:a07e8f090946 26 const double delta_t = 0.01;
kweisbeek 3:a07e8f090946 27 const double L1 = 370.0 / 2.0;
kweisbeek 3:a07e8f090946 28 const double L2 = 65.0/2.0;
kweisbeek 3:a07e8f090946 29 const double pi = 3.14159265359;
kweisbeek 3:a07e8f090946 30 const double alpha = (2.0 * pi) /(25.0*8400.0);
kweisbeek 3:a07e8f090946 31 const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
kweisbeek 6:23880486c8fc 32 double rotation_end_position=1;
kweisbeek 6:23880486c8fc 33 double tower_1_position=1;
kweisbeek 6:23880486c8fc 34 double tower_end_position=1;
kweisbeek 6:23880486c8fc 35 const double q1start = rotation_end_position * alpha;
kweisbeek 6:23880486c8fc 36 const double q2start = tower_1_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!
kweisbeek 6:23880486c8fc 37 const double q2end = tower_end_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!
kweisbeek 0:7789750c3b36 38
kweisbeek 3:a07e8f090946 39 //variables motors
kweisbeek 3:a07e8f090946 40 double out1;
kweisbeek 3:a07e8f090946 41 double out2;
kweisbeek 3:a07e8f090946 42 int counts1;
kweisbeek 3:a07e8f090946 43 int counts2;
kweisbeek 3:a07e8f090946 44 double vdesx;
kweisbeek 3:a07e8f090946 45 double vdesy;
kweisbeek 3:a07e8f090946 46 double q1;
kweisbeek 3:a07e8f090946 47 double q2;
kweisbeek 3:a07e8f090946 48 double MPe;
kweisbeek 3:a07e8f090946 49 double xe;
kweisbeek 3:a07e8f090946 50 double ye;
kweisbeek 3:a07e8f090946 51 double gamma;
kweisbeek 3:a07e8f090946 52 double dq1;
kweisbeek 3:a07e8f090946 53 double dq2;
kweisbeek 3:a07e8f090946 54 double dC1;
kweisbeek 3:a07e8f090946 55 double dC2;
kweisbeek 3:a07e8f090946 56 double pwm1;
kweisbeek 3:a07e8f090946 57 double pwm2;
kweisbeek 2:d41aa6d0e3cc 58
s1574396 5:0bbb52e29790 59 //PID rotation constants
kweisbeek 7:29c93152d96a 60 double Rot_Kp = 1.5;
kweisbeek 7:29c93152d96a 61 double Rot_Ki = 0.1;
kweisbeek 7:29c93152d96a 62 double Rot_Kd = 0.48;
s1574396 5:0bbb52e29790 63 double Rot_error = 0;
s1574396 5:0bbb52e29790 64 double Rot_prev_error = 0;
s1574396 5:0bbb52e29790 65
s1574396 5:0bbb52e29790 66 //PID translation constants
kweisbeek 7:29c93152d96a 67 const double Trans_Kp = 0.5;
kweisbeek 7:29c93152d96a 68 const double Trans_Ki = 0.5;
kweisbeek 7:29c93152d96a 69 const double Trans_Kd = 0.1;
s1574396 5:0bbb52e29790 70 double Trans_error = 0;
s1574396 5:0bbb52e29790 71 double Trans_prev_error = 0;
s1574396 5:0bbb52e29790 72
s1574396 5:0bbb52e29790 73 // PID execution
s1574396 5:0bbb52e29790 74 double PID_control(double error, const double kp, const double ki, const double kd, double &error_int, double &error_prev)
s1574396 5:0bbb52e29790 75 {
s1574396 5:0bbb52e29790 76 // P control
s1574396 5:0bbb52e29790 77 double u_k = kp * error;
s1574396 5:0bbb52e29790 78
s1574396 5:0bbb52e29790 79 // I control
s1574396 5:0bbb52e29790 80 error_int = error_int + (Ts * error);
s1574396 5:0bbb52e29790 81 double u_i = ki * error_int;
s1574396 5:0bbb52e29790 82
s1574396 5:0bbb52e29790 83 // D control
s1574396 5:0bbb52e29790 84 double error_deriv = (error - error_prev);
s1574396 5:0bbb52e29790 85 double u_d = kd * error_deriv;
s1574396 5:0bbb52e29790 86 error_prev = error;
s1574396 5:0bbb52e29790 87
s1574396 5:0bbb52e29790 88 return u_k + u_i + u_d;
s1574396 5:0bbb52e29790 89 }
s1574396 5:0bbb52e29790 90
kweisbeek 6:23880486c8fc 91 void boundaries(){
kweisbeek 6:23880486c8fc 92 double q2tot = q2 + dq2;
kweisbeek 6:23880486c8fc 93 if (q2tot > q2end){
kweisbeek 6:23880486c8fc 94 dq2 = 0;} //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
kweisbeek 6:23880486c8fc 95 else if (q2tot < q2start){
kweisbeek 6:23880486c8fc 96 dq2 = 0;}
kweisbeek 6:23880486c8fc 97 else{}
kweisbeek 6:23880486c8fc 98 }
kweisbeek 6:23880486c8fc 99
kweisbeek 3:a07e8f090946 100 void motor_control()
kweisbeek 3:a07e8f090946 101 {
kweisbeek 3:a07e8f090946 102 out1 = (pot1*2.0f)-1.0f; //control x-direction
kweisbeek 3:a07e8f090946 103 out2 = (pot2*2.0f)-1.0f; //control y-direction
kweisbeek 3:a07e8f090946 104 counts1 = encoder1.getPulses(); //counts encoder 1
kweisbeek 3:a07e8f090946 105 counts2 = encoder2.getPulses(); //counts encoder 2
kweisbeek 3:a07e8f090946 106 vdesx = out1 * 20.0; //speed x-direction
kweisbeek 3:a07e8f090946 107 vdesy = out2 * 20.0; //speed y-direction
kweisbeek 6:23880486c8fc 108
kweisbeek 6:23880486c8fc 109 q1 = counts1 * alpha + q1start; //counts to rotation (rad)
kweisbeek 6:23880486c8fc 110 q2 = counts2 * beta + q2start; //counts to translation (mm)
kweisbeek 3:a07e8f090946 111 MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
kweisbeek 3:a07e8f090946 112 xe = cos(q1) * MPe; //x location in frame 0
kweisbeek 3:a07e8f090946 113 ye = sin(q1) * MPe; //y location in frame 0
kweisbeek 3:a07e8f090946 114 gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
kweisbeek 3:a07e8f090946 115 dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
kweisbeek 3:a07e8f090946 116 dq2 = gamma * delta_t * (-1.0 * xe * vdesx - ye * vdesy); //target translation
kweisbeek 6:23880486c8fc 117 boundaries();
s1574396 5:0bbb52e29790 118 dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
s1574396 5:0bbb52e29790 119 dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
s1574396 4:d568f61be1d1 120 pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
s1574396 4:d568f61be1d1 121 pwm2 = 3.0 * (dC2 / delta_t) / 8400.0; //
kweisbeek 3:a07e8f090946 122 dirpin.write(pwm1 < 0);
s1574396 5:0bbb52e29790 123 pwmpin = fabs (pwm1);
kweisbeek 3:a07e8f090946 124 dirpin2.write(pwm2 < 0);
kweisbeek 3:a07e8f090946 125 pwmpin2 = fabs (pwm2);
kweisbeek 3:a07e8f090946 126 }
kweisbeek 6:23880486c8fc 127
kweisbeek 6:23880486c8fc 128
kweisbeek 0:7789750c3b36 129 //main
kweisbeek 0:7789750c3b36 130 int main(){
kweisbeek 0:7789750c3b36 131 pc.baud(115200);
kweisbeek 0:7789750c3b36 132 pc.printf("start\r\n");
kweisbeek 0:7789750c3b36 133 pwmpin.period_us(60);
kweisbeek 0:7789750c3b36 134
kweisbeek 0:7789750c3b36 135 while(1){
kweisbeek 3:a07e8f090946 136 motor_control();
s1574396 5:0bbb52e29790 137 pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
s1725696 1:125af627e307 138 wait(delta_t);
s1725696 1:125af627e307 139 }
s1574396 4:d568f61be1d1 140 }