Revision 1:1db5c1c28a93, committed 2016-06-23

Comitter:

vsluiter

Date:

Thu Jun 23 11:33:22 2016 +0000

Parent:

0:0b39982f89a9

Commit message:

New version with analog input

Changed in this revision

diff -r 0b39982f89a9 -r 1db5c1c28a93 main.cpp
--- a/main.cpp	Wed Sep 09 13:44:08 2015 +0000
+++ b/main.cpp	Thu Jun 23 11:33:22 2016 +0000
@@ -4,15 +4,42 @@
 
 Encoder motor1(D13,D12);
 PwmOut led(D9);
-HIDScope scope(1);
+HIDScope scope(3);
+
+
 
 int main()
 {
-    
+    const float degrees_per_count = 90/200.0; //quick approach
+    const float ain_degrees_range = 90; //degrees over which the potmeter can control.
+    const float kp_1_range = -1;
+    AnalogIn setpoint_1(A0);
+    AnalogIn p_value(A1);
+    DigitalOut direction_1(D4);
+    PwmOut pwm_1(D5);
+    float degrees_setpoint;
+    float error;
+    float output;
+    pwm_1.period(1.0/10000); //10kHz
+        
     while (true) {
+        degrees_setpoint = setpoint_1 * ain_degrees_range;
+        error = degrees_setpoint-(motor1.getPosition()*degrees_per_count); //error =  setpoint - current
+        output = error*p_value*kp_1_range;
+        if(output > 0)
+        {
+            direction_1.write(true);
+        }
+        else
+        {
+            direction_1.write(false);
+        }
+        pwm_1.write(fabs(output));
         scope.set(0,motor1.getPosition());
+        scope.set(1,error);
+        scope.set(2,fabs(output));
         led.write(motor1.getPosition()/100.0);
         scope.send();
-        wait(0.2f);
+        wait(0.01);
     }
 }
\ No newline at end of file