Biorobotics / Mbed 2 deprecated PID_Control

acos faalt

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@0:76bc19ed12ee, 2015-10-06 (annotated)

Committer:
davevogel0
Date:
Tue Oct 06 16:34:46 2015 +0000
Revision:
0:76bc19ed12ee
Child:
1:4465c9e203ce
Start from P 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 0:76bc19ed12ee 5
davevogel0 0:76bc19ed12ee 6 //---------- Change motor control parameters-------
davevogel0 0:76bc19ed12ee 7 float Ks = 0.5; //minimum power required to move
davevogel0 0:76bc19ed12ee 8 float Kp = 0.65; //strentgh of proportional control
davevogel0 0:76bc19ed12ee 9 float Kd = 0.12; //strentgh of differential control
davevogel0 0:76bc19ed12ee 10 float Ki = 0.5; //strentgh of integrational control
davevogel0 0:76bc19ed12ee 11 //----------End of control parameters
davevogel0 0:76bc19ed12ee 12
davevogel0 0:76bc19ed12ee 13
davevogel0 0:76bc19ed12ee 14
davevogel0 0:76bc19ed12ee 15 //encoder
davevogel0 0:76bc19ed12ee 16 QEI Motor1 (D13, D12, NC, 32);
davevogel0 0:76bc19ed12ee 17 QEI Motor2 (D11, D10, NC, 32);
davevogel0 0:76bc19ed12ee 18
davevogel0 0:76bc19ed12ee 19 //Define pins
davevogel0 0:76bc19ed12ee 20 DigitalOut M2(D7); //direction 2 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 21 PwmOut E2(D6); //speed 2
davevogel0 0:76bc19ed12ee 22 DigitalOut M1(D4); //direction 1 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 23 PwmOut E1(D5); //speed 1
davevogel0 0:76bc19ed12ee 24 AnalogIn pot1(A0); //read value of pot1 for position
davevogel0 0:76bc19ed12ee 25 AnalogIn pot2(A1); //read value of pot2 for position
davevogel0 0:76bc19ed12ee 26 DigitalOut myled(LED_GREEN);
davevogel0 0:76bc19ed12ee 27 MODSERIAL pc(USBTX, USBRX);
davevogel0 0:76bc19ed12ee 28 DigitalIn button(PTA4);
davevogel0 0:76bc19ed12ee 29 HIDScope scope(2);
davevogel0 0:76bc19ed12ee 30
davevogel0 0:76bc19ed12ee 31 //Define Variables
davevogel0 0:76bc19ed12ee 32 double Deg1;
davevogel0 0:76bc19ed12ee 33 double Deg2;
davevogel0 0:76bc19ed12ee 34 double M1error;
davevogel0 0:76bc19ed12ee 35 double M2error;
davevogel0 0:76bc19ed12ee 36 double M1output;
davevogel0 0:76bc19ed12ee 37 double M2output;
davevogel0 0:76bc19ed12ee 38 double pos1;
davevogel0 0:76bc19ed12ee 39 double pos2;
davevogel0 0:76bc19ed12ee 40 double output;
davevogel0 0:76bc19ed12ee 41 double pref1,pref2;
davevogel0 0:76bc19ed12ee 42 bool dir1;
davevogel0 0:76bc19ed12ee 43 bool dir2;
davevogel0 0:76bc19ed12ee 44 double long gearbox=0.08587786259541984732824427480916;
davevogel0 0:76bc19ed12ee 45
davevogel0 0:76bc19ed12ee 46 //Timers and Tickers
davevogel0 0:76bc19ed12ee 47 Timer t;
davevogel0 0:76bc19ed12ee 48 Ticker t1,t2,t3;
davevogel0 0:76bc19ed12ee 49
davevogel0 0:76bc19ed12ee 50 //booleans run program
davevogel0 0:76bc19ed12ee 51 volatile bool send_go = false, direction_go = false, speed_go = false;
davevogel0 0:76bc19ed12ee 52
davevogel0 0:76bc19ed12ee 53 //------------------Activate programs-----------
davevogel0 0:76bc19ed12ee 54 //Activate send data pc
davevogel0 0:76bc19ed12ee 55 void Send_True()
davevogel0 0:76bc19ed12ee 56 {
davevogel0 0:76bc19ed12ee 57 send_go = true;
davevogel0 0:76bc19ed12ee 58 }
davevogel0 0:76bc19ed12ee 59 // Activate desired location
davevogel0 0:76bc19ed12ee 60 void M_position_True()
davevogel0 0:76bc19ed12ee 61 {
davevogel0 0:76bc19ed12ee 62 direction_go = true;
davevogel0 0:76bc19ed12ee 63 }
davevogel0 0:76bc19ed12ee 64 //read position of motor and drive motor to set position.
davevogel0 0:76bc19ed12ee 65 void M_speed_True()
davevogel0 0:76bc19ed12ee 66 {
davevogel0 0:76bc19ed12ee 67 speed_go = true;
davevogel0 0:76bc19ed12ee 68 }
davevogel0 0:76bc19ed12ee 69 //------------------End of activate programs--------
davevogel0 0:76bc19ed12ee 70
davevogel0 0:76bc19ed12ee 71
davevogel0 0:76bc19ed12ee 72 //------------------Start of control programs-------
davevogel0 0:76bc19ed12ee 73
davevogel0 0:76bc19ed12ee 74 //Send values over HIDScope & Serial port
davevogel0 0:76bc19ed12ee 75 void Send()
davevogel0 0:76bc19ed12ee 76 {
davevogel0 0:76bc19ed12ee 77 scope.set(0,Motor1.getPulses());
davevogel0 0:76bc19ed12ee 78 scope.set(1,Motor2.getPulses());
davevogel0 0:76bc19ed12ee 79 scope.send();
davevogel0 0:76bc19ed12ee 80 pc.printf("Deg M1: %f M2: %f\n", Deg1, Deg2);
davevogel0 0:76bc19ed12ee 81 }
davevogel0 0:76bc19ed12ee 82
davevogel0 0:76bc19ed12ee 83 //Desired Position Motors --> future script emg
davevogel0 0:76bc19ed12ee 84 void direction()
davevogel0 0:76bc19ed12ee 85 {
davevogel0 0:76bc19ed12ee 86 pos1=pot1.read()*360-180;
davevogel0 0:76bc19ed12ee 87 pos2=pot2.read()*360-180;
davevogel0 0:76bc19ed12ee 88 pc.printf(" Desired: pos1: %f dir1: %i pos2: %f dir2: %i \n",pos1, dir1, pos2, dir2);
davevogel0 0:76bc19ed12ee 89 }
davevogel0 0:76bc19ed12ee 90
davevogel0 0:76bc19ed12ee 91 //translate desired position to movement motor
davevogel0 0:76bc19ed12ee 92 void speed()
davevogel0 0:76bc19ed12ee 93 {
davevogel0 0:76bc19ed12ee 94 //Read position --> Turn pulses to Degrees
davevogel0 0:76bc19ed12ee 95 Deg1=gearbox*Motor1.getPulses();
davevogel0 0:76bc19ed12ee 96 Deg2=gearbox*Motor2.getPulses();
davevogel0 0:76bc19ed12ee 97 //Direction the motors should turn
davevogel0 0:76bc19ed12ee 98 dir1=(Deg1 > pos1) ? false: true;
davevogel0 0:76bc19ed12ee 99 dir2=(Deg2 > pos1) ? false: true;
davevogel0 0:76bc19ed12ee 100 //Motor power
davevogel0 0:76bc19ed12ee 101 M1error=Ks+Kp*abs((Deg1-pos1)/pos1)+Kd*abs((pref1-abs((Deg1-pos1))/pos1))+Ki*0;
davevogel0 0:76bc19ed12ee 102 M2error=Ks+Kp*abs((Deg2-pos1)/pos1)+Kd*0+Ki*0;
davevogel0 0:76bc19ed12ee 103 pref1=abs((Deg1-pos1));//prefious error
davevogel0 0:76bc19ed12ee 104 pref2=abs((Deg2-pos2));//prefious error
davevogel0 0:76bc19ed12ee 105 pc.printf("M1error: %f\n", M1error);
davevogel0 0:76bc19ed12ee 106
davevogel0 0:76bc19ed12ee 107 //Motor controll
davevogel0 0:76bc19ed12ee 108 //Motor 1
davevogel0 0:76bc19ed12ee 109 if ((Deg1< pos1) || (Deg1> pos1)) {
davevogel0 0:76bc19ed12ee 110 E1=M1error;
davevogel0 0:76bc19ed12ee 111 M1=dir1;
davevogel0 0:76bc19ed12ee 112 pc.printf("M1>P1\n");
davevogel0 0:76bc19ed12ee 113 } else {
davevogel0 0:76bc19ed12ee 114 E1=0;
davevogel0 0:76bc19ed12ee 115 pc.printf("I am in Else\n");
davevogel0 0:76bc19ed12ee 116 }
davevogel0 0:76bc19ed12ee 117 //Motor 2
davevogel0 0:76bc19ed12ee 118 if ((Deg2< pos2) || (Deg2> pos2)) {
davevogel0 0:76bc19ed12ee 119 E2=M2error;
davevogel0 0:76bc19ed12ee 120 M2=dir2;
davevogel0 0:76bc19ed12ee 121 pc.printf("M2>P2\n");
davevogel0 0:76bc19ed12ee 122 } else {
davevogel0 0:76bc19ed12ee 123 E1=0;
davevogel0 0:76bc19ed12ee 124 pc.printf("I am in Else\n");
davevogel0 0:76bc19ed12ee 125 }
davevogel0 0:76bc19ed12ee 126 }
davevogel0 0:76bc19ed12ee 127 //--------------- End of control programs----------
davevogel0 0:76bc19ed12ee 128 int main()
davevogel0 0:76bc19ed12ee 129 {
davevogel0 0:76bc19ed12ee 130 //turn that led off!It hurts my eyes! Ow, I do boot.
davevogel0 0:76bc19ed12ee 131 myled=1;
davevogel0 0:76bc19ed12ee 132
davevogel0 0:76bc19ed12ee 133 //PWM period motors
davevogel0 0:76bc19ed12ee 134 E1.period(0.0001f);
davevogel0 0:76bc19ed12ee 135 E2.period(0.0001f);
davevogel0 0:76bc19ed12ee 136
davevogel0 0:76bc19ed12ee 137 pc.baud(115200);
davevogel0 0:76bc19ed12ee 138
davevogel0 0:76bc19ed12ee 139
davevogel0 0:76bc19ed12ee 140 //sub programs - time how fast everything occurs
davevogel0 0:76bc19ed12ee 141 t1.attach_us(&Send_True, 1e4); //Send data to pc
davevogel0 0:76bc19ed12ee 142 t2.attach_us(&M_position_True, 1e4); //Desired position motor(EMG goes here)
davevogel0 0:76bc19ed12ee 143 t3.attach_us(&M_speed_True, 1e4); //Speed control here
davevogel0 0:76bc19ed12ee 144
davevogel0 0:76bc19ed12ee 145
davevogel0 0:76bc19ed12ee 146 //-------------Scedule programs-------------------
davevogel0 0:76bc19ed12ee 147 while(1) {
davevogel0 0:76bc19ed12ee 148 if (send_go==true) {
davevogel0 0:76bc19ed12ee 149 Send();
davevogel0 0:76bc19ed12ee 150
davevogel0 0:76bc19ed12ee 151 send_go = false;
davevogel0 0:76bc19ed12ee 152 } else if (direction_go == true) {
davevogel0 0:76bc19ed12ee 153 direction();
davevogel0 0:76bc19ed12ee 154
davevogel0 0:76bc19ed12ee 155 direction_go = false;
davevogel0 0:76bc19ed12ee 156 } else if (speed_go == true) {
davevogel0 0:76bc19ed12ee 157 speed();
davevogel0 0:76bc19ed12ee 158
davevogel0 0:76bc19ed12ee 159 speed_go = false;
davevogel0 0:76bc19ed12ee 160 } else {
davevogel0 0:76bc19ed12ee 161
davevogel0 0:76bc19ed12ee 162 //-----------End of scedule progrmas----------------
davevogel0 0:76bc19ed12ee 163 }
davevogel0 0:76bc19ed12ee 164 }
davevogel0 0:76bc19ed12ee 165 }