Diff: main.cpp

Revision:

0:b781e625aca4

diff -r 000000000000 -r b781e625aca4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 27 20:38:26 2021 +0000
@@ -0,0 +1,68 @@
+// reads input voltage from distance measurement circuit and outputs to console
+
+#include "mbed.h"
+#include <iostream>
+
+//analog inputs to the microcontroller
+AnalogIn Lin(PTB2);
+AnalogIn Rin(PTB1);
+//pwm output to servo
+PwmOut output(PTB3);
+
+// ticker to sample error
+const float ti = 2e-2;
+const float PI = 3.1415926535;
+Ticker updater;
+
+//initializing variables
+float vL = 0;
+float vR = 0;
+float out = 0;
+float turn_angle = 0;
+float err = 0; 
+float prev_error = 0;
+float del_error = 0;
+int i = 0;
+float sample_count = 20;
+
+float sum = 0;
+
+// turning constants
+const float kp = 36.8;
+const float kd = 2.128;
+const float full_left = .059f; // duty cycle for full left turn
+const float full_right = .091f; // duty cycle for full right turn
+float d = full_left;
+
+void pwmUpdate()
+{
+    prev_error = err; 
+    vL = Lin.read(); 
+    vR = Rin.read();
+    
+    err = -(vL-vR) ; // voltage error
+    del_error = (err - prev_error)/ti; //change in voltage error
+    turn_angle = kp*err + kd*del_error; //calculates turn angle
+    if(turn_angle < 0) 
+        turn_angle = 0; 
+    if(turn_angle > PI) 
+        turn_angle = PI;  
+    d= (turn_angle/PI) * (full_right - full_left) + full_left;
+    if( abs(d - output.read()) > 0.01) //only updates the duty cycle if there is a change in the duty cycle greater than 0.3%
+        {
+         output.write(d);
+         }
+     cout << "del error: " << del_error << "\r" << std::endl;
+    
+}
+
+
+
+int main(void)
+{
+    output.period(0.02f); // 50 Hz control signal
+    // attaches pwmUpdate function to the ticker with a period of 20ms
+    updater.attach(&pwmUpdate,ti);
+    
+    while(1) {} // do nothing
+}
\ No newline at end of file