Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@1:f3fe6d2b7639, 2017-10-16 (annotated)

Committer:
MMartens
Date:
Mon Oct 16 09:43:57 2017 +0000
Revision:
1:f3fe6d2b7639
Parent:
0:9167ae5d9927
Child:
2:7c6391c8ca71
Weird behaviour at potvalue = 1; angle does not change

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 0:9167ae5d9927 5
MMartens 0:9167ae5d9927 6 MODSERIAL pc(USBTX,USBRX);
MMartens 0:9167ae5d9927 7 PwmOut speed1(D5);
MMartens 0:9167ae5d9927 8 DigitalOut dir1(D4);
MMartens 0:9167ae5d9927 9 DigitalOut led1(D8);
MMartens 0:9167ae5d9927 10 DigitalOut led2(D11);
MMartens 0:9167ae5d9927 11 AnalogIn pot(A1);
MMartens 0:9167ae5d9927 12 DigitalIn button1(D13);
MMartens 0:9167ae5d9927 13 DigitalIn button2(D12);
MMartens 0:9167ae5d9927 14 Ticker potmeterreadout;
MMartens 0:9167ae5d9927 15 Ticker encoderreadout;
MMartens 0:9167ae5d9927 16 Encoder motor1(PTD0,PTC4);
MMartens 0:9167ae5d9927 17
MMartens 1:f3fe6d2b7639 18 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 19
MMartens 0:9167ae5d9927 20 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 21 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 22
MMartens 1:f3fe6d2b7639 23 double setpoint = 180;//I am setting it to move through 180 degrees
MMartens 1:f3fe6d2b7639 24 double Kp = 0.15;// you can set these constants however you like depending on trial & error
MMartens 1:f3fe6d2b7639 25 double Ki = 1;
MMartens 1:f3fe6d2b7639 26 double Kd = 0.5;
MMartens 0:9167ae5d9927 27
MMartens 0:9167ae5d9927 28 float last_error = 0;
MMartens 1:f3fe6d2b7639 29 float error1 = 0;
MMartens 0:9167ae5d9927 30 float changeError = 0;
MMartens 0:9167ae5d9927 31 float totalError = 0;
MMartens 0:9167ae5d9927 32 float pidTerm = 0;
MMartens 0:9167ae5d9927 33 float pidTerm_scaled = 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 34
MMartens 0:9167ae5d9927 35 volatile double potvalue = 0.0;
MMartens 0:9167ae5d9927 36 volatile double position = 0.0;
MMartens 1:f3fe6d2b7639 37 /*void readpot()
MMartens 1:f3fe6d2b7639 38 {
MMartens 1:f3fe6d2b7639 39
MMartens 1:f3fe6d2b7639 40 }
MMartens 1:f3fe6d2b7639 41 */
MMartens 1:f3fe6d2b7639 42 void PIDcalculation()
MMartens 1:f3fe6d2b7639 43 {
MMartens 0:9167ae5d9927 44 potvalue = pot.read();
MMartens 1:f3fe6d2b7639 45 angle = motor1.getPosition()/4200.00*6.28;
MMartens 0:9167ae5d9927 46 setpoint = potvalue*setpoint;
MMartens 1:f3fe6d2b7639 47 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 48 //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 1:f3fe6d2b7639 49
MMartens 1:f3fe6d2b7639 50 if (angle <= setpoint) {
MMartens 1:f3fe6d2b7639 51 dir1 = 1;// Forward motion
MMartens 1:f3fe6d2b7639 52 } else {
MMartens 1:f3fe6d2b7639 53 dir1 = 0;//Reverse motion
MMartens 1:f3fe6d2b7639 54 }
MMartens 0:9167ae5d9927 55
MMartens 1:f3fe6d2b7639 56 error1 = setpoint - angle;
MMartens 1:f3fe6d2b7639 57 if (error1 <= 0.2) {
MMartens 1:f3fe6d2b7639 58 speed1 = 0;
MMartens 1:f3fe6d2b7639 59 }
MMartens 0:9167ae5d9927 60
MMartens 1:f3fe6d2b7639 61 changeError = error1 - last_error; // derivative term
MMartens 1:f3fe6d2b7639 62 totalError += error1; //accumalate errors to find integral term
MMartens 1:f3fe6d2b7639 63 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 1:f3fe6d2b7639 64 if (pidTerm >= 255) {
MMartens 1:f3fe6d2b7639 65 pidTerm = 255;
MMartens 1:f3fe6d2b7639 66 } else if (pidTerm <= -255) {
MMartens 1:f3fe6d2b7639 67 pidTerm = -255;
MMartens 1:f3fe6d2b7639 68 } else {
MMartens 1:f3fe6d2b7639 69 pidTerm = pidTerm;
MMartens 1:f3fe6d2b7639 70 }
MMartens 1:f3fe6d2b7639 71 //constraining to appropriate value
MMartens 1:f3fe6d2b7639 72 pidTerm_scaled = abs(pidTerm)/255;//make sure it's a positive value
MMartens 1:f3fe6d2b7639 73 last_error = error1;
MMartens 0:9167ae5d9927 74 }
MMartens 0:9167ae5d9927 75
MMartens 1:f3fe6d2b7639 76 int main()
MMartens 1:f3fe6d2b7639 77 {
MMartens 1:f3fe6d2b7639 78 encoderreadout.attach(PIDcalculation,0.001f);
MMartens 1:f3fe6d2b7639 79 speed1.period(PwmPeriod);
MMartens 1:f3fe6d2b7639 80
MMartens 1:f3fe6d2b7639 81 while(true) {
MMartens 1:f3fe6d2b7639 82
MMartens 0:9167ae5d9927 83
MMartens 1:f3fe6d2b7639 84
MMartens 1:f3fe6d2b7639 85 speed1 = pidTerm_scaled;
MMartens 0:9167ae5d9927 86
MMartens 1:f3fe6d2b7639 87 pc.printf("WHEEL ANGLE:%d, PID_scaled = %f, error = %f\r", angle, pidTerm_scaled, error1);
MMartens 0:9167ae5d9927 88
MMartens 1:f3fe6d2b7639 89 }
MMartens 1:f3fe6d2b7639 90
MMartens 0:9167ae5d9927 91 }
MMartens 0:9167ae5d9927 92
MMartens 0:9167ae5d9927 93