acos faalt
Dependencies: HIDScope MODSERIAL QEI mbed
main.cpp@6:05b6b70618db, 2015-10-21 (annotated)
- Committer:
- davevogel0
- Date:
- Wed Oct 21 21:00:48 2015 +0000
- Revision:
- 6:05b6b70618db
- Parent:
- 5:ec6dd614aa7e
Using the potmeter an angle is send out to the motors with a PID control.
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 | 2:da3b7dd2beb0 | 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 | 6:05b6b70618db | 12 | const double M1_friction = 0.4; //minimum power required to make the motor move |
davevogel0 | 1:4465c9e203ce | 13 | //Motor 2 |
davevogel0 | 6:05b6b70618db | 14 | const float M2_Ks = 0; //minimum power required to move |
davevogel0 | 6:05b6b70618db | 15 | const float M2_Kp = -10; //strentgh of proportional control |
davevogel0 | 6:05b6b70618db | 16 | const float M2_Kd = 0; //strentgh of differential control Include timestep in constant |
davevogel0 | 6:05b6b70618db | 17 | const float M2_Ki = 1; //strentgh of integrational control Include timestep in constant |
davevogel0 | 6:05b6b70618db | 18 | const double M2_friction = 0.4; //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 | 5:ec6dd614aa7e | 26 | DigitalOut M2(D7); //direction 2 //1 is ccw 0=cw |
davevogel0 | 5:ec6dd614aa7e | 27 | PwmOut E2(D6); //speed 2 |
davevogel0 | 5:ec6dd614aa7e | 28 | DigitalOut M1(D4); //direction 1 //1 is ccw 0=cw |
davevogel0 | 5:ec6dd614aa7e | 29 | PwmOut E1(D5); //speed 1 |
davevogel0 | 5:ec6dd614aa7e | 30 | AnalogIn pot1(A0); //read value of pot1 for position |
davevogel0 | 5:ec6dd614aa7e | 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 | 6:05b6b70618db | 38 | double M1_set, M1_error, M1_pos, M1_error_int, M1_error1; |
davevogel0 | 6:05b6b70618db | 39 | double M2_set, M2_error, M2_pos, M2_error_int, M2_error1; |
davevogel0 | 1:4465c9e203ce | 40 | double position; |
davevogel0 | 3:337345f748cf | 41 | const double long gearbox = 0.085877862; |
davevogel0 | 6:05b6b70618db | 42 | |
davevogel0 | 0:76bc19ed12ee | 43 | |
davevogel0 | 0:76bc19ed12ee | 44 | //Timers and Tickers |
davevogel0 | 0:76bc19ed12ee | 45 | Timer t; |
davevogel0 | 0:76bc19ed12ee | 46 | Ticker t1,t2,t3; |
davevogel0 | 0:76bc19ed12ee | 47 | |
davevogel0 | 0:76bc19ed12ee | 48 | //booleans run program |
davevogel0 | 6:05b6b70618db | 49 | volatile bool send_go = false, setpoint_go = false, control_go= false; |
davevogel0 | 0:76bc19ed12ee | 50 | |
davevogel0 | 0:76bc19ed12ee | 51 | //------------------Activate programs----------- |
davevogel0 | 0:76bc19ed12ee | 52 | //Activate send data pc |
davevogel0 | 3:337345f748cf | 53 | void Send_True() |
davevogel0 | 0:76bc19ed12ee | 54 | { |
davevogel0 | 0:76bc19ed12ee | 55 | send_go = true; |
davevogel0 | 0:76bc19ed12ee | 56 | } |
davevogel0 | 0:76bc19ed12ee | 57 | // Activate desired location |
davevogel0 | 1:4465c9e203ce | 58 | void M_Setpoint_True() |
davevogel0 | 0:76bc19ed12ee | 59 | { |
davevogel0 | 1:4465c9e203ce | 60 | setpoint_go = true; |
davevogel0 | 1:4465c9e203ce | 61 | } |
davevogel0 | 1:4465c9e203ce | 62 | // Controll if motor should go or not |
davevogel0 | 1:4465c9e203ce | 63 | void M_Control_True() |
davevogel0 | 1:4465c9e203ce | 64 | { |
davevogel0 | 1:4465c9e203ce | 65 | control_go = true; |
davevogel0 | 0:76bc19ed12ee | 66 | } |
davevogel0 | 0:76bc19ed12ee | 67 | //------------------End of activate programs-------- |
davevogel0 | 0:76bc19ed12ee | 68 | |
davevogel0 | 0:76bc19ed12ee | 69 | |
davevogel0 | 0:76bc19ed12ee | 70 | //------------------Start of control programs------- |
davevogel0 | 0:76bc19ed12ee | 71 | //Send values over HIDScope & Serial port |
davevogel0 | 0:76bc19ed12ee | 72 | void Send() |
davevogel0 | 0:76bc19ed12ee | 73 | { |
davevogel0 | 0:76bc19ed12ee | 74 | scope.set(0,Motor1.getPulses()); |
davevogel0 | 0:76bc19ed12ee | 75 | scope.set(1,Motor2.getPulses()); |
davevogel0 | 0:76bc19ed12ee | 76 | scope.send(); |
davevogel0 | 0:76bc19ed12ee | 77 | } |
davevogel0 | 0:76bc19ed12ee | 78 | |
davevogel0 | 3:337345f748cf | 79 | //Desired Position Motors |
davevogel0 | 6:05b6b70618db | 80 | void M_Setpoint() |
davevogel0 | 1:4465c9e203ce | 81 | { |
davevogel0 | 6:05b6b70618db | 82 | M1_set=pot1.read()*360-180; |
davevogel0 | 6:05b6b70618db | 83 | M2_set=pot2.read()*360-180; |
davevogel0 | 6:05b6b70618db | 84 | } |
davevogel0 | 2:da3b7dd2beb0 | 85 | |
davevogel0 | 6:05b6b70618db | 86 | // read position of motors |
davevogel0 | 6:05b6b70618db | 87 | void M_pos () |
davevogel0 | 6:05b6b70618db | 88 | { |
davevogel0 | 6:05b6b70618db | 89 | M1_pos = gearbox * Motor1.getPulses(); |
davevogel0 | 6:05b6b70618db | 90 | M2_pos = gearbox * Motor1.getPulses(); |
davevogel0 | 1:4465c9e203ce | 91 | } |
davevogel0 | 2:da3b7dd2beb0 | 92 | |
davevogel0 | 6:05b6b70618db | 93 | // calculate error between pos and setpoint |
davevogel0 | 6:05b6b70618db | 94 | void M_error () |
davevogel0 | 2:da3b7dd2beb0 | 95 | { |
davevogel0 | 6:05b6b70618db | 96 | M1_error= M1_set - M1_pos; |
davevogel0 | 6:05b6b70618db | 97 | M2_error= M2_set - M2_pos; |
davevogel0 | 2:da3b7dd2beb0 | 98 | } |
davevogel0 | 2:da3b7dd2beb0 | 99 | |
davevogel0 | 6:05b6b70618db | 100 | //translate error to power |
davevogel0 | 6:05b6b70618db | 101 | double Pw_control (double& S_error, double& S_error_int, double S_error1, double Ks, double Kp,double Ki,double Kd, double friction) |
davevogel0 | 3:337345f748cf | 102 | { |
davevogel0 | 1:4465c9e203ce | 103 | // Motor Power |
davevogel0 | 6:05b6b70618db | 104 | S_error_int = S_error_int + S_error / 1e4; |
davevogel0 | 1:4465c9e203ce | 105 | double Ps = Ks; |
davevogel0 | 6:05b6b70618db | 106 | double Pp = Kp * S_error; //Proportional control |
davevogel0 | 6:05b6b70618db | 107 | double Pi = Ki * S_error_int; //int controll |
davevogel0 | 6:05b6b70618db | 108 | double Pd = Kd * (S_error - S_error1); //Differtial control |
davevogel0 | 6:05b6b70618db | 109 | S_error1 = S_error; |
davevogel0 | 1:4465c9e203ce | 110 | double Power = Ps + Pp + Pi + Pd; |
davevogel0 | 1:4465c9e203ce | 111 | pc.printf("Power bf: %f Pi: %f \n", Power, Pi); |
davevogel0 | 1:4465c9e203ce | 112 | |
davevogel0 | 3:337345f748cf | 113 | // overcome minimum power required to turn and stop the motor from 'piepen' Also limit power to a max. |
davevogel0 | 1:4465c9e203ce | 114 | if (Power<friction) { |
davevogel0 | 1:4465c9e203ce | 115 | Power=0; |
davevogel0 | 3:337345f748cf | 116 | } else if (Power>0.7) { |
davevogel0 | 3:337345f748cf | 117 | Power=0.7; |
davevogel0 | 1:4465c9e203ce | 118 | } else {} |
davevogel0 | 1:4465c9e203ce | 119 | |
davevogel0 | 6:05b6b70618db | 120 | return Power ; |
davevogel0 | 1:4465c9e203ce | 121 | } |
davevogel0 | 3:337345f748cf | 122 | |
davevogel0 | 6:05b6b70618db | 123 | // direction control |
davevogel0 | 6:05b6b70618db | 124 | bool dr_control (double A, double B) |
davevogel0 | 5:ec6dd614aa7e | 125 | { |
davevogel0 | 6:05b6b70618db | 126 | int Direction = (A > B) ? false:true; |
davevogel0 | 6:05b6b70618db | 127 | return Direction; |
davevogel0 | 5:ec6dd614aa7e | 128 | } |
davevogel0 | 6:05b6b70618db | 129 | //--------------- End of control programs---------- |
davevogel0 | 5:ec6dd614aa7e | 130 | |
davevogel0 | 0:76bc19ed12ee | 131 | int main() |
davevogel0 | 0:76bc19ed12ee | 132 | { |
davevogel0 | 0:76bc19ed12ee | 133 | //turn that led off!It hurts my eyes! Ow, I do boot. |
davevogel0 | 2:da3b7dd2beb0 | 134 | myled = 1; |
davevogel0 | 0:76bc19ed12ee | 135 | |
davevogel0 | 0:76bc19ed12ee | 136 | //PWM period motors |
davevogel0 | 0:76bc19ed12ee | 137 | E1.period(0.0001f); |
davevogel0 | 0:76bc19ed12ee | 138 | E2.period(0.0001f); |
davevogel0 | 0:76bc19ed12ee | 139 | pc.baud(115200); |
davevogel0 | 0:76bc19ed12ee | 140 | |
davevogel0 | 0:76bc19ed12ee | 141 | //sub programs - time how fast everything occurs |
davevogel0 | 6:05b6b70618db | 142 | t1.attach_us(&Send_True, 1e4); //Send data to pc |
davevogel0 | 6:05b6b70618db | 143 | t2.attach_us(&M_Setpoint_True, 1e4); //Desired position motor |
davevogel0 | 6:05b6b70618db | 144 | t3.attach_us(M_Control_True, 1e4); //Speed control here |
davevogel0 | 5:ec6dd614aa7e | 145 | |
davevogel0 | 3:337345f748cf | 146 | //-------------Schedule programs------------------- |
davevogel0 | 0:76bc19ed12ee | 147 | while(1) { |
davevogel0 | 6:05b6b70618db | 148 | |
davevogel0 | 6:05b6b70618db | 149 | if(setpoint_go == true) { |
davevogel0 | 6:05b6b70618db | 150 | M_Setpoint(); |
davevogel0 | 5:ec6dd614aa7e | 151 | pc.printf("setpoint\n"); |
davevogel0 | 6:05b6b70618db | 152 | |
davevogel0 | 1:4465c9e203ce | 153 | setpoint_go = false; |
davevogel0 | 5:ec6dd614aa7e | 154 | } |
davevogel0 | 5:ec6dd614aa7e | 155 | if (control_go == true) { |
davevogel0 | 6:05b6b70618db | 156 | pc.printf("control:"); |
davevogel0 | 6:05b6b70618db | 157 | M_pos(); |
davevogel0 | 6:05b6b70618db | 158 | M_error(); |
davevogel0 | 6:05b6b70618db | 159 | //control Motor 1 |
davevogel0 | 6:05b6b70618db | 160 | E1 = Pw_control (M1_error, M1_error_int, M1_error1, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction); |
davevogel0 | 6:05b6b70618db | 161 | M1 = dr_control (M1_pos, M1_set); |
davevogel0 | 6:05b6b70618db | 162 | pc.printf("M1_power: %f M1_ Direction: ", E1, M1); |
davevogel0 | 6:05b6b70618db | 163 | // control Motor 2 |
davevogel0 | 6:05b6b70618db | 164 | E2 = Pw_control (M2_error, M2_error_int, M2_error1, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction); |
davevogel0 | 6:05b6b70618db | 165 | M2 = dr_control (M2_pos, M2_set); |
davevogel0 | 6:05b6b70618db | 166 | pc.printf("M2_power: %f M2_ Direction:", E2, M2); |
davevogel0 | 6:05b6b70618db | 167 | pc.printf("M1_error: %f M2_ error: \n", M1_error, M2_error); |
davevogel0 | 1:4465c9e203ce | 168 | control_go = false; |
davevogel0 | 5:ec6dd614aa7e | 169 | } |
davevogel0 | 5:ec6dd614aa7e | 170 | if (send_go == true) { |
davevogel0 | 5:ec6dd614aa7e | 171 | pc.printf("send\n"); |
davevogel0 | 0:76bc19ed12ee | 172 | Send(); |
davevogel0 | 0:76bc19ed12ee | 173 | |
davevogel0 | 0:76bc19ed12ee | 174 | send_go = false; |
davevogel0 | 5:ec6dd614aa7e | 175 | } |
davevogel0 | 0:76bc19ed12ee | 176 | //-----------End of scedule progrmas---------------- |
davevogel0 | 0:76bc19ed12ee | 177 | } |
davevogel0 | 6:05b6b70618db | 178 | } |