not working yet
Dependencies: Motor_with_encoder MODSERIAL mbed HIDScope
main.cpp
- Committer:
- vera1
- Date:
- 2017-10-23
- Revision:
- 6:fc46581fe3e0
- Parent:
- 5:8c6d66a7c5da
- Child:
- 7:809f122886ae
- Child:
- 8:9edf32e021a5
- Child:
- 18:5c4e27db4d9e
File content as of revision 6:fc46581fe3e0:
#include "mbed.h" #include "MODSERIAL.h" #include "encoder.h" #include "math.h" #include "HIDScope.h" //HIDScope scope(1); MODSERIAL pc(USBTX,USBRX); PwmOut speed1(D5); PwmOut speed2(D6); DigitalOut dir1(D4); DigitalOut dir2(D7); DigitalIn press(PTA4); DigitalOut led1(D8); DigitalOut led2(D11); AnalogIn pot(A2); AnalogIn pot2(A1); Ticker mainticker; Encoder motor1(PTD0,PTC4); Encoder motor2(D12,D13); float PwmPeriod = 0.0001f; double count = 0; //set the counts of the encoder volatile double angle = 0;//set the angles double count2 = 0; //set the counts of the encoder volatile double angle2 = 0;//set the angles double setpoint = 6.28;//I am setting it to move through 180 degrees double setpoint2 = 6.28;//I am setting it to move through 180 degrees double Kp = 250;// you can set these constants however you like depending on trial & error double Ki = 100; double Kd = 0; float last_error = 0; float error1 = 0; float changeError = 0; float totalError = 0; float pidTerm = 0; float pidTerm_scaled = 0; float last_error2 = 0; float error2 = 0; float changeError2 = 0; float totalError2 = 0; float pidTerm2 = 0; 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| volatile double potvalue = 0.0; volatile double potvalue2 = 0.0; volatile double position = 0.0; volatile double position2 = 0.0; //bool readoutsetpoint = true; void setpointreadout() { potvalue = pot.read(); setpoint = potvalue*6.28f; potvalue2 = pot2.read(); setpoint2 = potvalue2*6.28f; } void PIDcalculation() // inputs: potvalue, motor#, setpoint { setpointreadout(); angle = motor1.getPosition()/4200.00*6.28; angle2 = motor2.getPosition()/4200.00*6.28; //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint); //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); error1 = setpoint - angle; error2 = setpoint2 - angle2; changeError = (error1 - last_error)/0.001f; // derivative term totalError += error1*0.001f; //accumalate errors to find integral term pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain pidTerm = pidTerm; if (pidTerm >= 1000) { pidTerm = 1000; } else if (pidTerm <= -1000) { pidTerm = -1000; } else { pidTerm = pidTerm; } //constraining to appropriate value if (pidTerm >= 0) { dir1 = 1;// Forward motion } else { dir1 = 0;//Reverse motion } pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value if(pidTerm_scaled >= 0.55f){ pidTerm_scaled = 0.55f; } changeError2 = (error2 - last_error2)/0.001f; // derivative term totalError2 += error2*0.001f; //accumalate errors to find integral term pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain pidTerm2 = pidTerm2; if (pidTerm2 >= 1000) { pidTerm2 = 1000; } else if (pidTerm2 <= -1000) { pidTerm2 = -1000; } else { pidTerm2 = pidTerm2; } //constraining to appropriate value if (pidTerm2 >= 0) { dir2 = 1;// Forward motion } else { dir2 = 0;//Reverse motion } pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value if(pidTerm_scaled2 >= 0.55f){ pidTerm_scaled2 = 0.55f; } last_error = error1; speed1 = pidTerm_scaled+0.45f; last_error2 = error2; speed2 = pidTerm_scaled2+0.45f; } int main() { mainticker.attach(PIDcalculation,0.01f); speed1.period(PwmPeriod); speed2.period(PwmPeriod); int count = 0; while(true) { count++; if(count == 100){ count = 0; pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint); 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); } wait(0.001f); } }