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?
User | Revision | Line number | New 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 | } |