Diff: main.cpp

Revision:

8:ec3c634390c7

Parent:

7:a32766b96d91

Child:

9:ae548d05ec71

diff -r a32766b96d91 -r ec3c634390c7 main.cpp
--- a/main.cpp	Thu Sep 19 14:12:30 2019 +0000
+++ b/main.cpp	Fri Sep 20 09:20:00 2019 +0000
@@ -1,29 +1,49 @@
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
+#include "HIDScope.h"
+#include "QEI.h"
 #include "MODSERIAL.h"
 //#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "FastPWM.h"
 #include <iostream>
 MODSERIAL pc(USBTX, USBRX);
-DigitalOut ledred(LED_RED);
-DigitalOut ledblue(LED_BLUE);
-DigitalOut ledgreen(LED_GREEN);
-InterruptIn button(SW2);
-AnalogIn   ain(A0);
-DigitalOut dout(LED1);
+Encoder motor1(D13,D12);
+PwmOut led(D9);
+HIDScope scope(3);
  
-int main(void)
+ 
+ 
+int main()
 {
-    while (true) 
-    {
-        // test the voltage on the initialized analog pin
-        //  and if greater than 0.3 * VCC set the digital pin
-        //  to a logic 1 otherwise a logic 0
+    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
         
-        // print the percentage and 16 bit normalized values
-        pc.printf("percentage: %3.3f%%\n", ain.read()*100.0f);
-        pc.printf("normalized: 0x%04X \n", ain.read_u16());
-        wait_ms(1000);
+    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.01);
     }
 }