Working before header-files
Dependencies: Encoder FastPWM MODSERIAL Servo mbed
Fork of kinematics_control by
main.cpp
- Committer:
- DBerendsen
- Date:
- 2017-10-26
- Revision:
- 2:7e5ca3715fb6
- Parent:
- 1:570c0d599b9e
File content as of revision 2:7e5ca3715fb6:
#include "mbed.h" #include "encoder.h" #include "Servo.h" #include "MODSERIAL.h" #include "FastPWM.h" //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ MODSERIAL pc(USBTX, USBRX); Ticker MyControllerTicker1; Ticker MyControllerTicker2; const double RAD_PER_PULSE = 0.00074799825*2; const float CONTROLLER_TS = 0.02; const double PI = 3.14159265358979323846; //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //--------------------------------Servo----------------------------------------- //------------------------------------------------------------------------------ Servo MyServo(D9); InterruptIn But1(D8); int k=0; void servo_control (){ if (k==0){ MyServo = 0; k=1; } else{ MyServo = 2; k=0; } } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //-------------------------------Kinematics------------------------------------- //------------------------------------------------------------------------------ AnalogIn potmeter1(A3); AnalogIn potmeter2(A4); 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; 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 ; } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------PID_Controller----------------------------------- //------------------------------------------------------------------------------ double PID(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) { const double a1 = -4 / (N*Ts+2); const double a2 = -(N*Ts-2) / ( N*Ts+2); const double b0 = (4*Kp + 4*Kd*N +2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4); const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N) / (N*Ts + 2); const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4); double v = error - a1*v1 - a2*v2; double u = b0*v + b1*v1 + b2*v2; v2=v1; v1=v; return u; } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //--------------------------------Motor1---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor1(D5); DigitalOut motor1DirectionPin(D4); DigitalIn ENC2A(D12); DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); 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 position_alpha = RAD_PER_PULSE * encoder1.getPosition(); float error_alpha = reference_alpha-position_alpha; float magnitude1 = PID(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){ motor1DirectionPin = 1; } else{ motor1DirectionPin = 0; } } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //--------------------------------Motor2---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor2(D6); DigitalOut motor2DirectionPin(D7); DigitalIn ENC1A(D10); DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); 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 position_beta = RAD_PER_PULSE * -encoder2.getPosition(); float error_beta = reference_beta-position_beta; float magnitude2 = PID(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"); if (magnitude2 > 0){ motor2DirectionPin = 1; } else{ motor2DirectionPin = 0; } } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ int main(){ pc.baud(115200); motor1.period(0.0001f); motor2.period(0.0001f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); But1.rise(&servo_control); while(1) {} }