Revision 1:a35a32fa3ea4, committed 2016-02-01

Comitter:

manimani3712

Date:

Mon Feb 01 17:47:04 2016 +0000

Parent:

0:ef4f8ad78780

Commit message:

?????

Changed in this revision

--- a/main.cpp	Sun Jan 31 05:32:30 2016 +0000
+++ b/main.cpp	Mon Feb 01 17:47:04 2016 +0000
@@ -1,12 +1,20 @@
 #include "mbed.h"
+#define ON 1
+#define OFF 0
+#define Gy1_offset 0.01
 
-PwmOut pwm(PTB0);
-BusOut moter(PTB1,PTB2);
-BusOut LED(PTB3,PTC2);
+PwmOut pwm(PTD4);
+BusOut moter(PTE21,PTE22);
+BusOut LED(PTE23,PTE29);
 DigitalIn senser(PTC9);
+AnalogIn distance(PTC2);
+AnalogIn gy1_adc(PTB0);//ジャイロ1
+AnalogIn gy2_adc(PTB1);//ジャイロ2
+
 Serial pc(USBTX, USBRX);
 int fx00,f0x0,f00x,i=0,sw=0,on=0;
 float a;
+int n;
 
 int intcange(int x) {
     int y;
@@ -86,14 +94,61 @@
     }
 }
 
+double round3(double x){
+  double y;
+  y=double(int((x+0.005)*100))/100;
+  return(y);
+}
+
 int main() {
          pc.attach(numberget, Serial::RxIrq);
          float pwidth,duty;   
          pwm.period_ms(20);
          LED=0;
+         float distance_data;  
+           float gy1_data;
+  float gy1_center;
+  float gy1_sgm=0;
+  float gy1_wk0=0,gy1_wk1=0;
+  float gy1_avg=0,gy1_sum;
+  int i;
+ 
+  gy1_center=0;
+  for(i=0;i<10;i++){
+    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
+    wait(0.01);
+  }
+  gy1_center=round3(gy1_center/10);
+         
         while(1){
             moter=sw;
             LED=on;
+      distance_data=distance.read();
+      distance_data = distance_data*3.3;
+      distance_data = 12288/distance_data;
+      
+          gy1_sum=0;
+    for(i=0;i<10;i++){
+      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
+      wait(0.01);
+    }
+    gy1_avg=round3(gy1_sum/10);
+    
+    gy1_wk1=round3((gy1_avg-gy1_center)*1000/6.7);
+    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
+    gy1_wk0=gy1_wk1;
+    
+    if(gy1_sgm > 90){
+      gy1_sgm=90;  
+    }
+    else if(gy1_sgm < -90){
+      gy1_sgm=-90;
+    }
+    
+      if(n == 1) {
+      pc.printf("sensor:%2.5f\n",distance_data);
+      pc.printf("Deg:%2.0f",gy1_sgm);
+      }
            if(senser==0) {
                 LED=3;
                 i=0;