Revision 2:47ec66bbe8f9, committed 2017-10-11

Comitter:

peterknoben

Date:

Wed Oct 11 12:14:37 2017 +0000

Parent:

1:5681fcdc22fe

Commit message:

Angle control with servo

Changed in this revision

--- /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

--- 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