Wik de Haas
/
Aansturing_Groep_4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope mbed
Fork of
Aansturing_knoppen
by 
Wik de Haas
Diff: controller.h
- Revision:
- 0:55f9447aa02b
diff -r 000000000000 -r 55f9447aa02b controller.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.h Wed Oct 26 10:33:14 2016 +0000
@@ -0,0 +1,21 @@
+void controller_1()
+{
+double theta_1 = acos((sqrt(pow(x,2)+pow(yc,2))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
+double theta_2 = acos((x/L2)-cos(theta_1));
+
+double reference_1 = theta_1; //reference
+double plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
+double error_1 = reference_1 - plaats_1 ;
+double pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
+if (pwm_1<0){motor_1 = 1;}
+else {motor_1 = 0;}
+pwm_motor_1 = fabs(pwm_1);
+
+double reference_2 = theta_2; //reference
+double plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
+double error_2 = reference_2 - plaats_2 ;
+double pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 );
+if (pwm_2<0){motor_2 = 0;}
+else {motor_2 = 1;}
+pwm_motor_2 = fabs(pwm_2);
+}