Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@5:8c6d66a7c5da, 2017-10-20 (annotated)

Committer:
MMartens
Date:
Fri Oct 20 06:56:35 2017 +0000
Revision:
5:8c6d66a7c5da
Parent:
4:75f6e4845194
Child:
6:fc46581fe3e0
2 motors

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 0:9167ae5d9927 9 PwmOut speed1(D5);
MMartens 4:75f6e4845194 10 PwmOut speed2(D6);
MMartens 0:9167ae5d9927 11 DigitalOut dir1(D4);
MMartens 4:75f6e4845194 12 DigitalOut dir2(D7);
MMartens 4:75f6e4845194 13 DigitalIn but1(PTC6);
MMartens 0:9167ae5d9927 14 DigitalOut led1(D8);
MMartens 0:9167ae5d9927 15 DigitalOut led2(D11);
MMartens 4:75f6e4845194 16 AnalogIn pot(A2);
MMartens 4:75f6e4845194 17 AnalogIn pot2(A1);
MMartens 0:9167ae5d9927 18 Ticker potmeterreadout;
MMartens 0:9167ae5d9927 19 Ticker encoderreadout;
MMartens 3:e888f52a46bc 20 Ticker Frequency;
MMartens 0:9167ae5d9927 21 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 22 Encoder motor2(D12,D13);
MMartens 0:9167ae5d9927 23
MMartens 1:f3fe6d2b7639 24 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 25
MMartens 0:9167ae5d9927 26 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 27 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 28
MMartens 4:75f6e4845194 29 double count2 = 0; //set the counts of the encoder
MMartens 4:75f6e4845194 30 volatile double angle2 = 0;//set the angles
MMartens 4:75f6e4845194 31
MMartens 3:e888f52a46bc 32 double setpoint = 6.28;//I am setting it to move through 180 degrees
MMartens 4:75f6e4845194 33 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
MMartens 3:e888f52a46bc 34 double Kp = 250;// you can set these constants however you like depending on trial & error
MMartens 3:e888f52a46bc 35 double Ki = 100;
MMartens 2:7c6391c8ca71 36 double Kd = 0;
MMartens 0:9167ae5d9927 37
MMartens 0:9167ae5d9927 38 float last_error = 0;
MMartens 1:f3fe6d2b7639 39 float error1 = 0;
MMartens 0:9167ae5d9927 40 float changeError = 0;
MMartens 0:9167ae5d9927 41 float totalError = 0;
MMartens 0:9167ae5d9927 42 float pidTerm = 0;
MMartens 4:75f6e4845194 43 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 44
MMartens 4:75f6e4845194 45 float last_error2 = 0;
MMartens 4:75f6e4845194 46 float error2 = 0;
MMartens 4:75f6e4845194 47 float changeError2 = 0;
MMartens 4:75f6e4845194 48 float totalError2 = 0;
MMartens 4:75f6e4845194 49 float pidTerm2 = 0;
MMartens 4:75f6e4845194 50 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 51
MMartens 0:9167ae5d9927 52 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 53 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 54 volatile double position = 0.0;
MMartens 4:75f6e4845194 55 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 56
MMartens 5:8c6d66a7c5da 57 bool readoutsetpoint = true;
MMartens 3:e888f52a46bc 58
MMartens 5:8c6d66a7c5da 59 void setpointreadout()
MMartens 5:8c6d66a7c5da 60 {
MMartens 5:8c6d66a7c5da 61 if (readoutsetpoint == true){
MMartens 5:8c6d66a7c5da 62 potvalue = pot.read();
MMartens 5:8c6d66a7c5da 63 setpoint = potvalue*6.28f;
MMartens 5:8c6d66a7c5da 64
MMartens 5:8c6d66a7c5da 65 potvalue2 = pot2.read();
MMartens 5:8c6d66a7c5da 66 setpoint2 = potvalue2*6.28f;
MMartens 5:8c6d66a7c5da 67 }
MMartens 5:8c6d66a7c5da 68 else {
MMartens 5:8c6d66a7c5da 69 readoutsetpoint = false;
MMartens 5:8c6d66a7c5da 70 }
MMartens 5:8c6d66a7c5da 71 }
MMartens 4:75f6e4845194 72
MMartens 2:7c6391c8ca71 73
MMartens 4:75f6e4845194 74 void PIDcalculation() // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 75 {
MMartens 5:8c6d66a7c5da 76 setpointreadout();
MMartens 5:8c6d66a7c5da 77 angle = motor1.getPosition()/4200.00*6.28;
MMartens 4:75f6e4845194 78 angle2 = motor2.getPosition()/4200.00*6.28;
MMartens 5:8c6d66a7c5da 79
MMartens 1:f3fe6d2b7639 80 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 81 //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 2:7c6391c8ca71 82
MMartens 3:e888f52a46bc 83
MMartens 3:e888f52a46bc 84
MMartens 3:e888f52a46bc 85 error1 = setpoint - angle;
MMartens 4:75f6e4845194 86 error2 = setpoint2 - angle2;
MMartens 3:e888f52a46bc 87
MMartens 3:e888f52a46bc 88 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 89 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 90 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 91 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 92 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 93 pidTerm = 1000;
MMartens 3:e888f52a46bc 94 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 95 pidTerm = -1000;
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 3:e888f52a46bc 105 pidTerm_scaled = abs(pidTerm)/1000.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 4:75f6e4845194 112 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 113 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 114 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 115 pidTerm2 = 1000;
MMartens 4:75f6e4845194 116 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 117 pidTerm2 = -1000;
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 4:75f6e4845194 127 pidTerm_scaled2 = abs(pidTerm2)/1000.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 4:75f6e4845194 130 }
MMartens 3:e888f52a46bc 131
MMartens 1:f3fe6d2b7639 132 last_error = error1;
MMartens 3:e888f52a46bc 133 speed1 = pidTerm_scaled+0.45f;
MMartens 4:75f6e4845194 134 last_error2 = error2;
MMartens 4:75f6e4845194 135 speed2 = pidTerm_scaled2+0.45f;
MMartens 0:9167ae5d9927 136 }
MMartens 0:9167ae5d9927 137
MMartens 1:f3fe6d2b7639 138 int main()
MMartens 1:f3fe6d2b7639 139 {
MMartens 3:e888f52a46bc 140 encoderreadout.attach(PIDcalculation,0.01f);
MMartens 1:f3fe6d2b7639 141 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 142 speed2.period(PwmPeriod);
MMartens 3:e888f52a46bc 143
MMartens 3:e888f52a46bc 144 int count = 0;
MMartens 4:75f6e4845194 145
MMartens 4:75f6e4845194 146
MMartens 1:f3fe6d2b7639 147 while(true) {
MMartens 3:e888f52a46bc 148
MMartens 3:e888f52a46bc 149 count++;
MMartens 3:e888f52a46bc 150 if(count == 100){
MMartens 3:e888f52a46bc 151 count = 0;
MMartens 3:e888f52a46bc 152 pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
MMartens 4:75f6e4845194 153 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 154 }
MMartens 5:8c6d66a7c5da 155
MMartens 5:8c6d66a7c5da 156 if(but1 == 1){
MMartens 5:8c6d66a7c5da 157 setpoint = 0;
MMartens 5:8c6d66a7c5da 158 setpoint2 = 0;
MMartens 5:8c6d66a7c5da 159 while(error1 >= 0.1f && error2 >= 0.1f){
MMartens 5:8c6d66a7c5da 160
MMartens 5:8c6d66a7c5da 161 }
MMartens 5:8c6d66a7c5da 162 readoutsetpoint = true;
MMartens 5:8c6d66a7c5da 163 }
MMartens 4:75f6e4845194 164
MMartens 4:75f6e4845194 165
MMartens 4:75f6e4845194 166
MMartens 4:75f6e4845194 167
MMartens 3:e888f52a46bc 168 wait(0.001f);
MMartens 1:f3fe6d2b7639 169 }
MMartens 1:f3fe6d2b7639 170
MMartens 0:9167ae5d9927 171 }
MMartens 0:9167ae5d9927 172
MMartens 0:9167ae5d9927 173