message
Dependencies: Encoder FastPWM MODSERIAL Servo mbed
Fork of Angle_control_v3 by
Diff: main.cpp
- Revision:
- 2:47ec66bbe8f9
- Parent:
- 1:5681fcdc22fe
- Child:
- 3:1514725c8cc8
--- a/main.cpp Wed Oct 11 09:33:32 2017 +0000 +++ b/main.cpp Wed Oct 11 12:14:37 2017 +0000 @@ -1,10 +1,12 @@ #include "mbed.h" #include "encoder.h" +#include "Servo.h" -Ticker timer; +Ticker MyControllerTicker1; +Ticker MyControllerTicker2; const double PI = 3.141592653589793; const double RAD_PER_PULSE = 0.000476190; -float CONTROLLER_TS = 0.01; +const double CONTROLLER_TS = 0.01; //Motor1 @@ -32,65 +34,77 @@ double m2_err_int = 0; const double motor2_gain = 2*PI; +//Servo +Servo MyServo(D9); +InterruptIn But1(D8); +int k=0; -float getreferenceangle(const double PI, double potmeter) -{ + +float getreferenceangle(const double PI, double potmeter){ float max_angle = 2*PI; float reference_angle = max_angle * potmeter; return reference_angle; } -double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) -{ +double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) { e_int =+ Ts * error; return Kp * error + Ki * e_int ; } -void motor1_control(const double motor1_gain) -{ +void motor1_control(){ double referenceangle1 = getreferenceangle(PI, potmeter1); double position1 = RAD_PER_PULSE * encoder1.getPosition(); double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain; motor1 = fabs(magnitude1); - if (magnitude1 < 0) - { + if (magnitude1 < 0){ motor1DirectionPin = 1; } - else - { + else{ motor1DirectionPin = 0; } } -void motor2_control(const double motor2_gain) -{ + +void motor2_control(){ double referenceangle2 = getreferenceangle(PI, potmeter2); double position2 = RAD_PER_PULSE * encoder2.getPosition(); double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain; motor2 = fabs(magnitude2); - if (magnitude2 < 0) - { + if (magnitude2 < 0){ motor2DirectionPin = 1; } - else - { + else{ motor2DirectionPin = 0; } } +void servo_control (){ + if (k==0){ + MyServo = 0; + k=1; + } + else{ + MyServo = 2; + k=0; + } +} -int main() -{ - while(1) - { - motor1_control(motor1_gain); - wait(0.005f); - motor2_control(motor2_gain); - wait(0.005f); - } - - - -} \ No newline at end of file + +int main(){ + //motor1.period(0.01); + MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); + MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); + But1.rise(&servo_control); + + while(1) {} +} + +//{while(1) +// motor1_control(motor1_gain); +// wait(0.005f); +// motor2_control(motor2_gain); +// wait(0.005f); + // } + \ No newline at end of file