Biorobotics / Mbed 2 deprecated PID_Control

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?

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 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 }