Diff: main.cpp

Revision:

0:f3d65e123fda

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 09 01:03:04 2016 +0000
@@ -0,0 +1,95 @@
+#include "mbed.h"
+#include "TextLCD.h"
+//#include "I2cLCD.h"
+
+DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4);
+AnalogIn rotary1(p15);
+Serial pc(USBTX,USBRX); //(USBTX,USBRX)
+Timer t1,t2;
+InterruptIn ch1(p16),ch2(p17);
+PwmOut yawyaw(p21),ch5out(p22);
+
+double t1_pwm;
+double t2_pwm;
+
+/////////////timer//////////////
+void t1_start(){
+  t1.start();
+}
+
+void t1_stop (){
+  t1.stop();
+   t1_pwm  =   t1.read();
+   t1.reset();
+}
+
+void t2_start(){
+  t2.start();
+}
+
+void t2_stop (){
+  t2.stop();
+   t2_pwm  =   t2.read();
+   t2.reset();
+}
+
+///////////////////////////////////
+
+
+
+
+int main(void){
+    
+    pc.baud(115200);
+
+    double yaw_gain_p = 0.001;
+    double yaw_s;
+    
+    ch1.rise(&t1_start) ;
+    ch1.fall(&t1_stop) ;
+     
+    ch2.rise(&t2_start) ;
+    ch2.fall(&t2_stop) ;
+    
+         
+    yawyaw.period(0.020);
+    
+    double rot_read = rotary1.read();
+    
+   
+        while(1){
+
+        
+            if (t2_pwm > 0.0015){
+
+                double yaw_error = (rotary1.read() - 0.4785);
+        
+                yaw_s = 0.00148 + yaw_gain_p * yaw_error;
+                
+                     if (yaw_s > 0.00175){yaw_s = 0.00175;}
+                     if (yaw_s < 0.0012){yaw_s = 0.0012;}
+                         
+        
+                //if (t2_pwm > 0.0015){
+                yawyaw.pulsewidth(yaw_s); // servo position determined by a pulsewidth between 1-2ms
+                
+                }
+    
+        
+            else{
+                yawyaw.pulsewidth(t1_pwm);}
+        
+                
+                 //lcd.locate(8,1);
+                //rot_read = (rotary1.read())*1000000;
+                //pc.printf("rotary1 = %f\r\n", rotary1.read());
+                pc.printf("yaw_s = %f\r\n", yaw_s);
+                pc.printf("t1_pwm = %f\r\n", t1_pwm);
+                pc.printf("t2_pwm = %f\r\n", t2_pwm);
+                //wait(1);
+                ch5out.pulsewidth(t2_pwm);
+        }
+    
+}
+    
+