Biorobotics / Mbed 2 deprecated PID_Control

acos faalt

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@1:4465c9e203ce, 2015-10-15 (annotated)

Committer:
davevogel0
Date:
Thu Oct 15 10:42:30 2015 +0000
Revision:
1:4465c9e203ce
Parent:
0:76bc19ed12ee
Child:
2:da3b7dd2beb0
PID funtions, calibration point almost done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
davevogel0 0:76bc19ed12ee 1 #include "mbed.h"
davevogel0 0:76bc19ed12ee 2 #include "HIDScope.h"
davevogel0 0:76bc19ed12ee 3 #include "MODSERIAL.h"
davevogel0 0:76bc19ed12ee 4 #include "QEI.h"
davevogel0 0:76bc19ed12ee 5
davevogel0 0:76bc19ed12ee 6 //---------- Change motor control parameters-------
davevogel0 1:4465c9e203ce 7 //Motor 1
davevogel0 1:4465c9e203ce 8 const float M1_Ks = 0.4; //minimum power required to move
davevogel0 1:4465c9e203ce 9 const float M1_Kp = 0.1; //strentgh of proportional control
davevogel0 1:4465c9e203ce 10 const float M1_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 11 const float M1_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 12 const double M1_friction = 0.55; //minimum power required to make the motor move
davevogel0 1:4465c9e203ce 13 //Motor 2
davevogel0 1:4465c9e203ce 14 const float M2_Ks = 0.4; //minimum power required to move
davevogel0 1:4465c9e203ce 15 const float M2_Kp = 0.1; //strentgh of proportional control
davevogel0 1:4465c9e203ce 16 const float M2_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 17 const float M2_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 18 const double M2_friction = 0.55; //minimum power required to make the motor move
davevogel0 0:76bc19ed12ee 19 //----------End of control parameters
davevogel0 0:76bc19ed12ee 20
davevogel0 0:76bc19ed12ee 21 //encoder
davevogel0 0:76bc19ed12ee 22 QEI Motor1 (D13, D12, NC, 32);
davevogel0 0:76bc19ed12ee 23 QEI Motor2 (D11, D10, NC, 32);
davevogel0 0:76bc19ed12ee 24
davevogel0 0:76bc19ed12ee 25 //Define pins
davevogel0 0:76bc19ed12ee 26 DigitalOut M2(D7); //direction 2 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 27 PwmOut E2(D6); //speed 2
davevogel0 0:76bc19ed12ee 28 DigitalOut M1(D4); //direction 1 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 29 PwmOut E1(D5); //speed 1
davevogel0 0:76bc19ed12ee 30 AnalogIn pot1(A0); //read value of pot1 for position
davevogel0 0:76bc19ed12ee 31 AnalogIn pot2(A1); //read value of pot2 for position
davevogel0 0:76bc19ed12ee 32 DigitalOut myled(LED_GREEN);
davevogel0 0:76bc19ed12ee 33 MODSERIAL pc(USBTX, USBRX);
davevogel0 0:76bc19ed12ee 34 DigitalIn button(PTA4);
davevogel0 0:76bc19ed12ee 35 HIDScope scope(2);
davevogel0 0:76bc19ed12ee 36
davevogel0 0:76bc19ed12ee 37 //Define Variables
davevogel0 1:4465c9e203ce 38 double M1_dpos, M1_pos;
davevogel0 1:4465c9e203ce 39 double M2_dpos, M2_pos;
davevogel0 1:4465c9e203ce 40 double M_error, M1_error, M2_error;
davevogel0 1:4465c9e203ce 41 double position;
davevogel0 1:4465c9e203ce 42 double pref,pref2;
davevogel0 1:4465c9e203ce 43 bool M1_dir, M2_dir;
davevogel0 1:4465c9e203ce 44 const double long gearbox=0.08587786259541984732824427480916;
davevogel0 1:4465c9e203ce 45 double M_error1 = 0;
davevogel0 0:76bc19ed12ee 46
davevogel0 0:76bc19ed12ee 47 //Timers and Tickers
davevogel0 0:76bc19ed12ee 48 Timer t;
davevogel0 0:76bc19ed12ee 49 Ticker t1,t2,t3;
davevogel0 0:76bc19ed12ee 50
davevogel0 0:76bc19ed12ee 51 //booleans run program
davevogel0 1:4465c9e203ce 52 volatile bool send_go = false, setpoint_go = false, speed_go = false, control_go= false;
davevogel0 0:76bc19ed12ee 53
davevogel0 0:76bc19ed12ee 54 //------------------Activate programs-----------
davevogel0 0:76bc19ed12ee 55 //Activate send data pc
davevogel0 1:4465c9e203ce 56 void D_Send_True()
davevogel0 0:76bc19ed12ee 57 {
davevogel0 0:76bc19ed12ee 58 send_go = true;
davevogel0 0:76bc19ed12ee 59 }
davevogel0 0:76bc19ed12ee 60 // Activate desired location
davevogel0 1:4465c9e203ce 61 void M_Setpoint_True()
davevogel0 0:76bc19ed12ee 62 {
davevogel0 1:4465c9e203ce 63 setpoint_go = true;
davevogel0 1:4465c9e203ce 64 }
davevogel0 1:4465c9e203ce 65 // Controll if motor should go or not
davevogel0 1:4465c9e203ce 66 void M_Control_True()
davevogel0 1:4465c9e203ce 67 {
davevogel0 1:4465c9e203ce 68 control_go = true;
davevogel0 0:76bc19ed12ee 69 }
davevogel0 0:76bc19ed12ee 70 //read position of motor and drive motor to set position.
davevogel0 1:4465c9e203ce 71 void M_Speed_True()
davevogel0 0:76bc19ed12ee 72 {
davevogel0 0:76bc19ed12ee 73 speed_go = true;
davevogel0 0:76bc19ed12ee 74 }
davevogel0 0:76bc19ed12ee 75 //------------------End of activate programs--------
davevogel0 0:76bc19ed12ee 76
davevogel0 0:76bc19ed12ee 77
davevogel0 0:76bc19ed12ee 78 //------------------Start of control programs-------
davevogel0 0:76bc19ed12ee 79
davevogel0 0:76bc19ed12ee 80 //Send values over HIDScope & Serial port
davevogel0 0:76bc19ed12ee 81 void Send()
davevogel0 0:76bc19ed12ee 82 {
davevogel0 0:76bc19ed12ee 83 scope.set(0,Motor1.getPulses());
davevogel0 0:76bc19ed12ee 84 scope.set(1,Motor2.getPulses());
davevogel0 0:76bc19ed12ee 85 scope.send();
davevogel0 1:4465c9e203ce 86 // pc.printf("Deg M1: %f M2: %f\n", M1_pos, M2_pos);
davevogel0 0:76bc19ed12ee 87 }
davevogel0 0:76bc19ed12ee 88
davevogel0 0:76bc19ed12ee 89 //Desired Position Motors --> future script emg
davevogel0 1:4465c9e203ce 90 void Setpoint()
davevogel0 1:4465c9e203ce 91 {
davevogel0 1:4465c9e203ce 92 M1_dpos=pot1.read()*360-180;
davevogel0 1:4465c9e203ce 93 M2_dpos=pot2.read()*360-180;
davevogel0 1:4465c9e203ce 94 pc.printf(" Desired: M1_dpos: %f M1_dir: %i M2_dpos: %f M2_dir: %i \n",M1_dpos, M1_dir, M2_dpos, M2_dir);
davevogel0 1:4465c9e203ce 95 }
davevogel0 1:4465c9e203ce 96 double Pw_control (double Pulse,double setpoint,double Ks, double Kp,double Ki,double Kd, double friction)
davevogel0 0:76bc19ed12ee 97 {
davevogel0 1:4465c9e203ce 98 //read position motor
davevogel0 1:4465c9e203ce 99 position = gearbox*Pulse;
davevogel0 1:4465c9e203ce 100 // Motor Power
davevogel0 1:4465c9e203ce 101 M_error = abs(setpoint-position);
davevogel0 1:4465c9e203ce 102 double M_error_Int= M_error_Int+M_error/1e4;
davevogel0 1:4465c9e203ce 103 double Ps = Ks;
davevogel0 1:4465c9e203ce 104 double Pp = Kp * M_error; //Proportional control
davevogel0 1:4465c9e203ce 105 double Pi = Ki * M_error_Int*M_error; //check dit --> nog niet goed //int controll
davevogel0 1:4465c9e203ce 106 double Pd = Kd * (M_error-M_error1); //Differtial control
davevogel0 1:4465c9e203ce 107 M_error1 = M_error;
davevogel0 1:4465c9e203ce 108 double Power = Ps + Pp + Pi + Pd;
davevogel0 1:4465c9e203ce 109 pc.printf("Power bf: %f Pi: %f \n", Power, Pi);
davevogel0 1:4465c9e203ce 110
davevogel0 1:4465c9e203ce 111 // overcome minimum power required to turn and stop the motor from 'piepen'.
davevogel0 1:4465c9e203ce 112 if (Power<friction) {
davevogel0 1:4465c9e203ce 113 Power=0;
davevogel0 1:4465c9e203ce 114 } else {}
davevogel0 1:4465c9e203ce 115
davevogel0 1:4465c9e203ce 116 //print error
davevogel0 1:4465c9e203ce 117 // pc.printf("Error: %f Direction: %f Position M: %f Setpoint %f \n", M_error, position, setpoint);
davevogel0 1:4465c9e203ce 118
davevogel0 1:4465c9e203ce 119 pc.printf("Power: %f\n", Power);
davevogel0 1:4465c9e203ce 120 return Power ;//is it possible to return 2 things for
davevogel0 1:4465c9e203ce 121 }
davevogel0 1:4465c9e203ce 122 // direction controll
davevogel0 1:4465c9e203ce 123 bool Dr_control (double Pulse,double setpoint)
davevogel0 1:4465c9e203ce 124 {
davevogel0 1:4465c9e203ce 125 //read position motor
davevogel0 1:4465c9e203ce 126 double position = gearbox*Pulse;
davevogel0 1:4465c9e203ce 127 // Direction motor should turn
davevogel0 1:4465c9e203ce 128 int Direction = (position > setpoint) ? false:true;
davevogel0 1:4465c9e203ce 129 return Direction;
davevogel0 1:4465c9e203ce 130 }
davevogel0 1:4465c9e203ce 131 // begin to set point
davevogel0 1:4465c9e203ce 132 void calibration()
davevogel0 1:4465c9e203ce 133 {
davevogel0 1:4465c9e203ce 134 M1_dpos=0;
davevogel0 1:4465c9e203ce 135 M2_dpos=0;
davevogel0 1:4465c9e203ce 136 if (button==true) {
davevogel0 1:4465c9e203ce 137 M1_pos=0;
davevogel0 1:4465c9e203ce 138 M2_pos=0;
davevogel0 1:4465c9e203ce 139 } else {
davevogel0 1:4465c9e203ce 140 }
davevogel0 0:76bc19ed12ee 141 }
davevogel0 0:76bc19ed12ee 142
davevogel0 0:76bc19ed12ee 143
davevogel0 0:76bc19ed12ee 144 //--------------- End of control programs----------
davevogel0 0:76bc19ed12ee 145 int main()
davevogel0 0:76bc19ed12ee 146 {
davevogel0 0:76bc19ed12ee 147 //turn that led off!It hurts my eyes! Ow, I do boot.
davevogel0 0:76bc19ed12ee 148 myled=1;
davevogel0 0:76bc19ed12ee 149
davevogel0 0:76bc19ed12ee 150 //PWM period motors
davevogel0 0:76bc19ed12ee 151 E1.period(0.0001f);
davevogel0 0:76bc19ed12ee 152 E2.period(0.0001f);
davevogel0 0:76bc19ed12ee 153
davevogel0 0:76bc19ed12ee 154 pc.baud(115200);
davevogel0 0:76bc19ed12ee 155
davevogel0 0:76bc19ed12ee 156
davevogel0 0:76bc19ed12ee 157 //sub programs - time how fast everything occurs
davevogel0 1:4465c9e203ce 158 t1.attach_us(&D_Send_True, 1e4); //Send data to pc
davevogel0 1:4465c9e203ce 159 t2.attach_us(&M_Setpoint_True, 1e4); //Desired position motor(EMG goes here)
davevogel0 1:4465c9e203ce 160 t3.attach_us(M_Control_True, 1e4); //Speed control here
davevogel0 0:76bc19ed12ee 161
davevogel0 0:76bc19ed12ee 162
davevogel0 0:76bc19ed12ee 163 //-------------Scedule programs-------------------
davevogel0 0:76bc19ed12ee 164 while(1) {
davevogel0 1:4465c9e203ce 165 if (setpoint_go == true) {
davevogel0 1:4465c9e203ce 166 Setpoint();
davevogel0 1:4465c9e203ce 167
davevogel0 1:4465c9e203ce 168 setpoint_go = false;
davevogel0 1:4465c9e203ce 169 } else if (control_go == true) {
davevogel0 1:4465c9e203ce 170 // control Motor1
davevogel0 1:4465c9e203ce 171 E1 = Pw_control (Motor1.getPulses(), M1_dpos, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
davevogel0 1:4465c9e203ce 172 M1 = Dr_control (Motor1.getPulses(), M1_dpos);
davevogel0 1:4465c9e203ce 173 M1_pos = position;
davevogel0 1:4465c9e203ce 174 M1_error = M_error;
davevogel0 1:4465c9e203ce 175 // control Motor2
davevogel0 1:4465c9e203ce 176 E2 = Pw_control (Motor2.getPulses(), M2_dpos, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
davevogel0 1:4465c9e203ce 177 //M2 = Dr_control (Motor2.getPulses(),M2_dpos);
davevogel0 1:4465c9e203ce 178 M2_pos = position;
davevogel0 1:4465c9e203ce 179 M2_error = M_error;
davevogel0 1:4465c9e203ce 180 //print error
davevogel0 1:4465c9e203ce 181 pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error); // once this works place it in 'send'
davevogel0 1:4465c9e203ce 182 control_go = false;
davevogel0 1:4465c9e203ce 183 } else if (send_go==true) {
davevogel0 0:76bc19ed12ee 184 Send();
davevogel0 0:76bc19ed12ee 185
davevogel0 0:76bc19ed12ee 186 send_go = false;
davevogel0 0:76bc19ed12ee 187 } else {
davevogel0 0:76bc19ed12ee 188
davevogel0 0:76bc19ed12ee 189 //-----------End of scedule progrmas----------------
davevogel0 0:76bc19ed12ee 190 }
davevogel0 0:76bc19ed12ee 191 }
davevogel0 0:76bc19ed12ee 192 }