Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@19:8746a2c663f8, 2017-11-01 (annotated)

Committer:
MMartens
Date:
Wed Nov 01 15:39:46 2017 +0000
Revision:
19:8746a2c663f8
Parent:
18:5c4e27db4d9e 
pid control;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MMartens 0:9167ae5d9927 1 #include "mbed.h"
MMartens 0:9167ae5d9927 2 #include "MODSERIAL.h"
MMartens 0:9167ae5d9927 3 #include "encoder.h"
MMartens 0:9167ae5d9927 4 #include "math.h"
MMartens 3:e888f52a46bc 5 #include "HIDScope.h"
MMartens 0:9167ae5d9927 6
MMartens 3:e888f52a46bc 7 //HIDScope scope(1);
MMartens 0:9167ae5d9927 8 MODSERIAL pc(USBTX,USBRX);
MMartens 18:5c4e27db4d9e 9 AnalogOut servospeed();
MMartens 0:9167ae5d9927 10 PwmOut speed1(D5);
MMartens 4:75f6e4845194 11 PwmOut speed2(D6);
MMartens 0:9167ae5d9927 12 DigitalOut dir1(D4);
MMartens 4:75f6e4845194 13 DigitalOut dir2(D7);
vera1 6:fc46581fe3e0 14 DigitalIn press(PTA4);
MMartens 0:9167ae5d9927 15 DigitalOut led1(D8);
MMartens 0:9167ae5d9927 16 DigitalOut led2(D11);
MMartens 4:75f6e4845194 17 AnalogIn pot(A2);
MMartens 18:5c4e27db4d9e 18 AnalogIn pot2(A3);
vera1 6:fc46581fe3e0 19 Ticker mainticker;
MMartens 0:9167ae5d9927 20 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 21 Encoder motor2(D12,D13);
MMartens 0:9167ae5d9927 22
MMartens 1:f3fe6d2b7639 23 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 24
MMartens 0:9167ae5d9927 25 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 26 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 27
MMartens 4:75f6e4845194 28 double count2 = 0; //set the counts of the encoder
MMartens 4:75f6e4845194 29 volatile double angle2 = 0;//set the angles
MMartens 4:75f6e4845194 30
MMartens 18:5c4e27db4d9e 31 double setpoint = 0;//I am setting it to move through 180 degrees
MMartens 18:5c4e27db4d9e 32 double setpoint2 = 0;//I am setting it to move through 180 degrees
MMartens 18:5c4e27db4d9e 33
MMartens 18:5c4e27db4d9e 34 double Kp = 15.25;// you can set these constants however you like depending on trial & error
MMartens 18:5c4e27db4d9e 35 double Ki = 0.0;
MMartens 18:5c4e27db4d9e 36 double Kd = 0.0;
MMartens 18:5c4e27db4d9e 37
MMartens 18:5c4e27db4d9e 38 double Kp2 = 30;
MMartens 18:5c4e27db4d9e 39 double Ki2 = 30;
MMartens 18:5c4e27db4d9e 40 double Kd2 = 2;
MMartens 0:9167ae5d9927 41
MMartens 0:9167ae5d9927 42 float last_error = 0;
MMartens 1:f3fe6d2b7639 43 float error1 = 0;
MMartens 0:9167ae5d9927 44 float changeError = 0;
MMartens 0:9167ae5d9927 45 float totalError = 0;
MMartens 0:9167ae5d9927 46 float pidTerm = 0;
MMartens 4:75f6e4845194 47 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 48
MMartens 4:75f6e4845194 49 float last_error2 = 0;
MMartens 4:75f6e4845194 50 float error2 = 0;
MMartens 4:75f6e4845194 51 float changeError2 = 0;
MMartens 4:75f6e4845194 52 float totalError2 = 0;
MMartens 4:75f6e4845194 53 float pidTerm2 = 0;
MMartens 4:75f6e4845194 54 float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
MMartens 0:9167ae5d9927 55
MMartens 0:9167ae5d9927 56 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 57 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 58 volatile double position = 0.0;
MMartens 4:75f6e4845194 59 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 60
vera1 6:fc46581fe3e0 61 //bool readoutsetpoint = true;
MMartens 3:e888f52a46bc 62
MMartens 5:8c6d66a7c5da 63 void setpointreadout()
MMartens 5:8c6d66a7c5da 64 {
vera1 6:fc46581fe3e0 65
MMartens 5:8c6d66a7c5da 66 potvalue = pot.read();
MMartens 18:5c4e27db4d9e 67 setpoint = potvalue*6.28f*15.0f;
MMartens 5:8c6d66a7c5da 68
MMartens 18:5c4e27db4d9e 69 //potvalue2 = pot2.read();
MMartens 18:5c4e27db4d9e 70 setpoint2 = potvalue*6.28f*15.0f;
vera1 6:fc46581fe3e0 71
MMartens 5:8c6d66a7c5da 72 }
MMartens 4:75f6e4845194 73
MMartens 2:7c6391c8ca71 74
MMartens 4:75f6e4845194 75 void PIDcalculation() // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 76 {
MMartens 5:8c6d66a7c5da 77 setpointreadout();
MMartens 5:8c6d66a7c5da 78 angle = motor1.getPosition()/4200.00*6.28;
MMartens 4:75f6e4845194 79 angle2 = motor2.getPosition()/4200.00*6.28;
MMartens 5:8c6d66a7c5da 80
MMartens 1:f3fe6d2b7639 81 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 82 //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
MMartens 19:8746a2c663f8 83
MMartens 3:e888f52a46bc 84 error1 = setpoint - angle;
MMartens 4:75f6e4845194 85 error2 = setpoint2 - angle2;
vera1 6:fc46581fe3e0 86
MMartens 18:5c4e27db4d9e 87 changeError = (error1 - last_error)/0.002f; // derivative term
MMartens 18:5c4e27db4d9e 88 totalError += error1*0.002f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 89 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 90 pidTerm = pidTerm;
MMartens 18:5c4e27db4d9e 91
MMartens 18:5c4e27db4d9e 92 if (pidTerm >= 100) {
MMartens 18:5c4e27db4d9e 93 pidTerm = 100;
MMartens 18:5c4e27db4d9e 94 } else if (pidTerm <= -100) {
MMartens 18:5c4e27db4d9e 95 pidTerm = -100;
MMartens 3:e888f52a46bc 96 } else {
MMartens 3:e888f52a46bc 97 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 98 }
MMartens 3:e888f52a46bc 99 //constraining to appropriate value
MMartens 3:e888f52a46bc 100 if (pidTerm >= 0) {
MMartens 1:f3fe6d2b7639 101 dir1 = 1;// Forward motion
MMartens 1:f3fe6d2b7639 102 } else {
MMartens 1:f3fe6d2b7639 103 dir1 = 0;//Reverse motion
MMartens 1:f3fe6d2b7639 104 }
MMartens 18:5c4e27db4d9e 105 pidTerm_scaled = abs(pidTerm)/100.0f; //make sure it's a positive value
MMartens 3:e888f52a46bc 106 if(pidTerm_scaled >= 0.55f){
MMartens 3:e888f52a46bc 107 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 108 }
MMartens 4:75f6e4845194 109
MMartens 4:75f6e4845194 110 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 111 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 18:5c4e27db4d9e 112 pidTerm2 = (Kp2 * error2) + (Ki2 * totalError2) + (Kd2 * changeError2);//total gain
MMartens 4:75f6e4845194 113 pidTerm2 = pidTerm2;
MMartens 18:5c4e27db4d9e 114 if (pidTerm2 >= 100) {
MMartens 18:5c4e27db4d9e 115 pidTerm2 = 100;
MMartens 18:5c4e27db4d9e 116 } else if (pidTerm2 <= -100) {
MMartens 18:5c4e27db4d9e 117 pidTerm2 = -100;
MMartens 4:75f6e4845194 118 } else {
MMartens 4:75f6e4845194 119 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 120 }
MMartens 4:75f6e4845194 121 //constraining to appropriate value
MMartens 4:75f6e4845194 122 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 123 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 124 } else {
MMartens 4:75f6e4845194 125 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 126 }
MMartens 18:5c4e27db4d9e 127 pidTerm_scaled2 = abs(pidTerm2)/100.0f; //make sure it's a positive value
MMartens 4:75f6e4845194 128 if(pidTerm_scaled2 >= 0.55f){
MMartens 4:75f6e4845194 129 pidTerm_scaled2 = 0.55f;
MMartens 18:5c4e27db4d9e 130
MMartens 4:75f6e4845194 131 }
MMartens 3:e888f52a46bc 132
MMartens 1:f3fe6d2b7639 133 last_error = error1;
MMartens 4:75f6e4845194 134 last_error2 = error2;
MMartens 18:5c4e27db4d9e 135
MMartens 18:5c4e27db4d9e 136
MMartens 18:5c4e27db4d9e 137 speed1 = pidTerm_scaled+0.45f;
MMartens 18:5c4e27db4d9e 138
MMartens 18:5c4e27db4d9e 139 speed2 = pidTerm_scaled2+0.45f;
MMartens 18:5c4e27db4d9e 140
MMartens 18:5c4e27db4d9e 141
MMartens 0:9167ae5d9927 142 }
MMartens 0:9167ae5d9927 143
MMartens 1:f3fe6d2b7639 144 int main()
MMartens 1:f3fe6d2b7639 145 {
MMartens 18:5c4e27db4d9e 146 mainticker.attach(PIDcalculation,0.002f);
MMartens 1:f3fe6d2b7639 147 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 148 speed2.period(PwmPeriod);
MMartens 3:e888f52a46bc 149
vera1 6:fc46581fe3e0 150 int count = 0;
MMartens 1:f3fe6d2b7639 151 while(true) {
MMartens 3:e888f52a46bc 152
MMartens 3:e888f52a46bc 153 count++;
MMartens 3:e888f52a46bc 154 if(count == 100){
MMartens 3:e888f52a46bc 155 count = 0;
MMartens 3:e888f52a46bc 156 pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
MMartens 18:5c4e27db4d9e 157 //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
MMartens 3:e888f52a46bc 158 }
MMartens 5:8c6d66a7c5da 159
vera1 6:fc46581fe3e0 160
vera1 6:fc46581fe3e0 161 wait(0.001f);
vera1 6:fc46581fe3e0 162 }
MMartens 4:75f6e4845194 163
MMartens 4:75f6e4845194 164
MMartens 4:75f6e4845194 165
MMartens 4:75f6e4845194 166
vera1 6:fc46581fe3e0 167
vera1 6:fc46581fe3e0 168
MMartens 1:f3fe6d2b7639 169
MMartens 0:9167ae5d9927 170 }
MMartens 0:9167ae5d9927 171
MMartens 0:9167ae5d9927 172