Diff: main.cpp

Revision:

14:90ac96893fcd

Parent:

13:e41d32a48931

Child:

15:1d21cf90cd47

diff -r c1e72469eea4 -r 90ac96893fcd main.cpp
--- a/main.cpp	Mon Apr 10 20:28:55 2017 +0000
+++ b/main.cpp	Mon Apr 10 21:00:30 2017 +0000
@@ -15,6 +15,7 @@
 
 PwmOut P1(PE_9);
 DigitalOut EN1(D0);
+AnalogIn I1(A0);
 
 //PwmOut P2(PE_11);     1D FOCUS FOR NOW
 //PwmOut P3(PE_13);
@@ -31,6 +32,8 @@
 double Kbt = 5.4;
 double Kbv = 0.33;
 double Kwv = 0.124;
+
+double cur1;
 double r1;
 
 int isPressed;
@@ -38,8 +41,12 @@
 void pwmupdate() {
     
     myled = !myled;
+    cur1 = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    cur1 = (cur1-2.0)*5000.0; // Change the value to be in the 0 to 3300 range
     
-    r1 = (Kbt*euler_angles.h*3.14/180.0);
+    
+    r1 = (Kbt*euler_angles.h);
+    
     r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
     
     //Limit PWM range
@@ -97,7 +104,7 @@
     while(1) {
 
         
-        pc.printf("Heading:%+6.4f [deg], R1%+6.4f [PWM] \r\n", //, Pitch:%+6.4f [deg]\r\n",
+        pc.printf("Heading:%+6.4f [rad], R1%+6.4f [PWM] \r\n", //, Pitch:%+6.4f [deg]\r\n",
                     euler_angles.h, r1, euler_angles.p);
         
         imu.get_Euler_Angles(&euler_angles);