![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sdfa
Dependencies: AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed
Fork of kinematics_controlv2 by
Diff: main.cpp
- Revision:
- 1:406519ff0f17
- Parent:
- 0:5f4bc2d63814
- Child:
- 3:c768a37620c9
--- a/main.cpp Thu Oct 19 19:42:08 2017 +0000 +++ b/main.cpp Thu Oct 26 10:54:02 2017 +0000 @@ -1,34 +1,45 @@ +#include "AnglePosition.h" +#include "PIDControl.h" #include "mbed.h" #include "encoder.h" #include "Servo.h" #include "MODSERIAL.h" - - - - +#include "FastPWM.h" //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ -MODSERIAL pc(USBTX, USBRX); -Ticker MyControllerTicker1; -Ticker MyControllerTicker2; +MODSERIAL pc(USBTX, USBRX); // +Ticker MyControllerTicker1; //Declare Ticker Motor 1 +Ticker MyControllerTicker2; //Declare Ticker Motor 2 +AnglePosition Angle; //Declare Angle calculater +PIDControl PID; //Declare PID Controller +AnalogIn potmeter1(A3); //Set Inputpin +AnalogIn potmeter2(A4); //Set Inputpin + +const float CONTROLLER_TS = 0.02; //Motor frequency + +//------------------------------------------------------------------------------ +//-------------------------Kinematic Constants---------------------------------- +//------------------------------------------------------------------------------ const double RAD_PER_PULSE = 0.00074799825*2; -const float CONTROLLER_TS = 0.5; const double PI = 3.14159265358979323846; -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - - +const float max_rangex = 500.0; +const float max_rangey = 300.0; +const float x_offset = 156.15; +const float y_offset = -76.97; +const float alpha_offset = -(21.52/180)*PI; +const float beta_offset = 0.0; +const float L1 = 450.0; +const float L2 = 490.0; //------------------------------------------------------------------------------ //--------------------------------Servo----------------------------------------- //------------------------------------------------------------------------------ -Servo MyServo(D9); -InterruptIn But1(D8); -int k=0; +Servo MyServo(D9); //Declare button +InterruptIn But1(D8); //Declare used button +int k=0; //Position flag void servo_control (){ if (k==0){ @@ -38,144 +49,71 @@ else{ MyServo = 2; k=0; - } -} -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - - - - - -//------------------------------------------------------------------------------ -//-------------------------------Kinematics------------------------------------- -//------------------------------------------------------------------------------ -AnalogIn potmeter1(A3); -AnalogIn potmeter2(A4); -const float max_rangex = 300.0; -const float max_rangey = 300.0; -const float x_offset = 156.15; -const float y_offset = -76.97; -const float alpha_offset = -(21.52/180)*PI; -const float beta_offset = 0.0; -const float L1 = 450.0; -const float L2 = 490.0; - -float gettargetposition(double potmeter, int max_range){ - float target = potmeter * max_range; - return target; -} - -float getreferenceposition(float target, float offset) { - float referenceposition = target + offset; - return referenceposition; + } } -float getreferencealpha(float max_rangex, float maxrangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2) { - float x_target = gettargetposition(potmeter1, max_rangex); - float y_target = gettargetposition(potmeter2, max_rangey); - float x = getreferenceposition(x_target, x_offset); - float y = getreferenceposition(y_target, y_offset); - float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) )); - float alpha = alpha_ + alpha_offset; - return alpha; -} - -float getreferencebeta(float max_rangex, float maxrangey, float x_offset, float y_offset, float beta_offset, float L1, float L2) { - float x_target = gettargetposition(potmeter1, max_rangex); - float y_target = gettargetposition(potmeter2, max_rangey); - float x = getreferenceposition(x_target, x_offset); - float y = getreferenceposition(y_target, y_offset); - float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) )); - float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset; - return beta; -} -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - - - - - -//------------------------------------------------------------------------------ -//------------------------------PI_Controller----------------------------------- -//------------------------------------------------------------------------------ -float PI_controller(float error, const float Kp, const float Ki, float Ts, double &e_int) { - e_int += Ts * error; - return Kp * error + Ki * e_int ; -} -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - - - - //------------------------------------------------------------------------------ //--------------------------------Motor1---------------------------------------- //------------------------------------------------------------------------------ -PwmOut motor1(D5); +FastPWM motor1(D5); DigitalOut motor1DirectionPin(D4); DigitalIn ENC2A(D12); DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); -const float MOTOR1_KP = 20.0; -const float MOTOR1_KI = 10.0; -double m1_err_int = 0; +const float MOTOR1_KP = 40.0; +const float MOTOR1_KI = 0.0; +const float MOTOR1_KD = 15.0; +double M1_v1 = 0.0; +double M1_v2 = 0.0; const double motor1_gain = 2*PI; +const float M1_N = 0.5; void motor1_control(){ - float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2); + float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2); float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); float error_alpha = reference_alpha-position_alpha; - float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ motor1_gain; + float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; motor1 = fabs(magnitude1); pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1); pc.printf("\r\n"); - - - if (magnitude1 < 0){ + // Determine Motor Direction + if (magnitude1 < 0){ motor1DirectionPin = 1; } else{ motor1DirectionPin = 0; } } -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - - - - //------------------------------------------------------------------------------ //--------------------------------Motor2---------------------------------------- //------------------------------------------------------------------------------ -PwmOut motor2(D6); +FastPWM motor2(D6); DigitalOut motor2DirectionPin(D7); DigitalIn ENC1A(D10); DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); -const float MOTOR2_KP = 20.0; -const float MOTOR2_KI = 10.0; +const float MOTOR2_KP = 60.0; +const float MOTOR2_KI = 0.0; +const float MOTOR2_KD = 15.0; double m2_err_int = 0; const double motor2_gain = 2*PI; +const float M2_N = 0.5; +double M2_v1 = 0.0; +double M2_v2 = 0.0; void motor2_control(){ - float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2); + float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2); float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); float error_beta = reference_beta-position_beta; - float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ motor2_gain; + float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; motor2 = fabs(magnitude2); pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2); pc.printf("\r\n"); - + //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; } @@ -187,21 +125,14 @@ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ - - - - int main(){ pc.baud(115200); - motor1.period(0.0002f); - motor2.period(0.0002f); + motor1.period(0.0001f); + motor2.period(0.0001f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); - But1.rise(&servo_control); - - - - - + But1.rise(&servo_control); while(1) {} -} \ No newline at end of file +} + +