Angle control and Servo control

Dependencies:   Encoder Servo mbed

Fork of Angle_control by Dustin Berendsen

Files at this revision

API Documentation at this revision

Comitter:
peterknoben
Date:
Wed Oct 11 12:14:37 2017 +0000
Parent:
1:5681fcdc22fe
Commit message:
Angle control with servo

Changed in this revision

Servo.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 5681fcdc22fe -r 47ec66bbe8f9 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Oct 11 12:14:37 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 5681fcdc22fe -r 47ec66bbe8f9 main.cpp
--- 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