Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@6:fc46581fe3e0, 2017-10-23 (annotated)

Committer:
vera1
Date:
Mon Oct 23 07:48:56 2017 +0000
Revision:
6:fc46581fe3e0
Parent:
5:8c6d66a7c5da
Child:
7:809f122886ae
Child:
8:9edf32e021a5
Child:
18:5c4e27db4d9e
3 motors working with setpoint of potmeter

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);
vera1 6:fc46581fe3e0 13 DigitalIn press(PTA4);
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);
vera1 6:fc46581fe3e0 18 Ticker mainticker;
MMartens 0:9167ae5d9927 19 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 20 Encoder motor2(D12,D13);
MMartens 0:9167ae5d9927 21
MMartens 1:f3fe6d2b7639 22 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 23
MMartens 0:9167ae5d9927 24 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 25 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 26
MMartens 4:75f6e4845194 27 double count2 = 0; //set the counts of the encoder
MMartens 4:75f6e4845194 28 volatile double angle2 = 0;//set the angles
MMartens 4:75f6e4845194 29
MMartens 3:e888f52a46bc 30 double setpoint = 6.28;//I am setting it to move through 180 degrees
MMartens 4:75f6e4845194 31 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
MMartens 3:e888f52a46bc 32 double Kp = 250;// you can set these constants however you like depending on trial & error
MMartens 3:e888f52a46bc 33 double Ki = 100;
MMartens 2:7c6391c8ca71 34 double Kd = 0;
MMartens 0:9167ae5d9927 35
MMartens 0:9167ae5d9927 36 float last_error = 0;
MMartens 1:f3fe6d2b7639 37 float error1 = 0;
MMartens 0:9167ae5d9927 38 float changeError = 0;
MMartens 0:9167ae5d9927 39 float totalError = 0;
MMartens 0:9167ae5d9927 40 float pidTerm = 0;
MMartens 4:75f6e4845194 41 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 42
MMartens 4:75f6e4845194 43 float last_error2 = 0;
MMartens 4:75f6e4845194 44 float error2 = 0;
MMartens 4:75f6e4845194 45 float changeError2 = 0;
MMartens 4:75f6e4845194 46 float totalError2 = 0;
MMartens 4:75f6e4845194 47 float pidTerm2 = 0;
MMartens 4:75f6e4845194 48 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 49
MMartens 0:9167ae5d9927 50 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 51 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 52 volatile double position = 0.0;
MMartens 4:75f6e4845194 53 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 54
vera1 6:fc46581fe3e0 55 //bool readoutsetpoint = true;
MMartens 3:e888f52a46bc 56
MMartens 5:8c6d66a7c5da 57 void setpointreadout()
MMartens 5:8c6d66a7c5da 58 {
vera1 6:fc46581fe3e0 59
MMartens 5:8c6d66a7c5da 60 potvalue = pot.read();
MMartens 5:8c6d66a7c5da 61 setpoint = potvalue*6.28f;
MMartens 5:8c6d66a7c5da 62
MMartens 5:8c6d66a7c5da 63 potvalue2 = pot2.read();
MMartens 5:8c6d66a7c5da 64 setpoint2 = potvalue2*6.28f;
vera1 6:fc46581fe3e0 65
MMartens 5:8c6d66a7c5da 66 }
MMartens 4:75f6e4845194 67
MMartens 2:7c6391c8ca71 68
MMartens 4:75f6e4845194 69 void PIDcalculation() // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 70 {
MMartens 5:8c6d66a7c5da 71 setpointreadout();
MMartens 5:8c6d66a7c5da 72 angle = motor1.getPosition()/4200.00*6.28;
MMartens 4:75f6e4845194 73 angle2 = motor2.getPosition()/4200.00*6.28;
MMartens 5:8c6d66a7c5da 74
MMartens 1:f3fe6d2b7639 75 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 76 //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 77
MMartens 3:e888f52a46bc 78 error1 = setpoint - angle;
MMartens 4:75f6e4845194 79 error2 = setpoint2 - angle2;
vera1 6:fc46581fe3e0 80
MMartens 3:e888f52a46bc 81 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 82 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 83 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 84 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 85 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 86 pidTerm = 1000;
MMartens 3:e888f52a46bc 87 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 88 pidTerm = -1000;
MMartens 3:e888f52a46bc 89 } else {
MMartens 3:e888f52a46bc 90 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 91 }
MMartens 3:e888f52a46bc 92 //constraining to appropriate value
MMartens 3:e888f52a46bc 93 if (pidTerm >= 0) {
MMartens 1:f3fe6d2b7639 94 dir1 = 1;// Forward motion
MMartens 1:f3fe6d2b7639 95 } else {
MMartens 1:f3fe6d2b7639 96 dir1 = 0;//Reverse motion
MMartens 1:f3fe6d2b7639 97 }
MMartens 3:e888f52a46bc 98 pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
MMartens 3:e888f52a46bc 99 if(pidTerm_scaled >= 0.55f){
MMartens 3:e888f52a46bc 100 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 101 }
MMartens 4:75f6e4845194 102
MMartens 4:75f6e4845194 103 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 104 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 4:75f6e4845194 105 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 106 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 107 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 108 pidTerm2 = 1000;
MMartens 4:75f6e4845194 109 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 110 pidTerm2 = -1000;
MMartens 4:75f6e4845194 111 } else {
MMartens 4:75f6e4845194 112 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 113 }
MMartens 4:75f6e4845194 114 //constraining to appropriate value
MMartens 4:75f6e4845194 115 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 116 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 117 } else {
MMartens 4:75f6e4845194 118 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 119 }
MMartens 4:75f6e4845194 120 pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
MMartens 4:75f6e4845194 121 if(pidTerm_scaled2 >= 0.55f){
MMartens 4:75f6e4845194 122 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 123 }
MMartens 3:e888f52a46bc 124
MMartens 1:f3fe6d2b7639 125 last_error = error1;
MMartens 3:e888f52a46bc 126 speed1 = pidTerm_scaled+0.45f;
MMartens 4:75f6e4845194 127 last_error2 = error2;
MMartens 4:75f6e4845194 128 speed2 = pidTerm_scaled2+0.45f;
MMartens 0:9167ae5d9927 129 }
MMartens 0:9167ae5d9927 130
MMartens 1:f3fe6d2b7639 131 int main()
MMartens 1:f3fe6d2b7639 132 {
vera1 6:fc46581fe3e0 133 mainticker.attach(PIDcalculation,0.01f);
MMartens 1:f3fe6d2b7639 134 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 135 speed2.period(PwmPeriod);
MMartens 3:e888f52a46bc 136
vera1 6:fc46581fe3e0 137 int count = 0;
MMartens 1:f3fe6d2b7639 138 while(true) {
MMartens 3:e888f52a46bc 139
MMartens 3:e888f52a46bc 140 count++;
MMartens 3:e888f52a46bc 141 if(count == 100){
MMartens 3:e888f52a46bc 142 count = 0;
MMartens 3:e888f52a46bc 143 pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
MMartens 4:75f6e4845194 144 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 145 }
MMartens 5:8c6d66a7c5da 146
vera1 6:fc46581fe3e0 147
vera1 6:fc46581fe3e0 148 wait(0.001f);
vera1 6:fc46581fe3e0 149 }
MMartens 4:75f6e4845194 150
MMartens 4:75f6e4845194 151
MMartens 4:75f6e4845194 152
MMartens 4:75f6e4845194 153
vera1 6:fc46581fe3e0 154
vera1 6:fc46581fe3e0 155
MMartens 1:f3fe6d2b7639 156
MMartens 0:9167ae5d9927 157 }
MMartens 0:9167ae5d9927 158
MMartens 0:9167ae5d9927 159