![](/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
main.cpp
- Committer:
- peterknoben
- Date:
- 2017-10-26
- Revision:
- 1:406519ff0f17
- Parent:
- 0:5f4bc2d63814
- Child:
- 3:c768a37620c9
File content as of revision 1:406519ff0f17:
#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; //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 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); //Declare button InterruptIn But1(D8); //Declare used button int k=0; //Position flag void servo_control (){ if (k==0){ MyServo = 0; k=1; } else{ MyServo = 2; k=0; } } //------------------------------------------------------------------------------ //--------------------------------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 = 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 = 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"); // Determine Motor Direction 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 = 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 = 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; } 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) {} }