Diff: main.cpp

Revision:

0:43eac3418598

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 20 11:49:26 2011 +0000
@@ -0,0 +1,142 @@
+// Gyro test07
+// Akidukidensi AE-GYRO-SMD
+// with 2Gyro+2Servo
+#include "mbed.h"
+#include "TextLCD0420.h"
+
+#define ON 1
+#define OFF 0
+#define Gy1_offset 0.01
+
+DigitalOut mled0(LED1);
+DigitalOut mled1(LED2);
+AnalogIn gy1_adc(p16);
+AnalogIn gy2_adc(p17);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+
+TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
+
+double round3(double x){
+  double y;
+  y=double(int((x+0.005)*100))/100;
+  return(y);
+}
+
+int main() {
+  float gy1_center;
+  float gy1_sgm=0;
+  float gy1_wk0=0,gy1_wk1=0;
+  float gy1_avg=0,gy1_sum;
+  int i,gy1_lc=9,gy1_lc_old=0;
+  float pwidth1;
+  float gy2_center;
+  float gy2_sgm=0;
+  float gy2_wk0=0,gy2_wk1=0;
+  float gy2_avg=0,gy2_sum;
+  int gy2_lc=9,gy2_lc_old=0;
+  float pwidth2;
+  
+  lcd.cls();
+  lcd.locate(0,0);
+  lcd.printf("*** Gyro test07 ****\n");
+  servo1.period_ms(20);
+  servo2.period_ms(20);
+
+  gy1_center=0;
+  gy2_center=0;
+  for(i=0;i<300;i++){
+    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
+    gy2_center=gy2_center+round3(gy2_adc.read()*3.3);
+    wait(0.001);
+  }
+  gy1_center=round3(gy1_center/300);
+  gy2_center=round3(gy2_center/300);
+  servo1.pulsewidth(0.0015);
+  servo2.pulsewidth(0.0015);
+  
+  while(1){
+    gy1_sum=0;
+    gy2_sum=0;
+    for(i=0;i<20;i++){
+      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
+      gy2_sum=gy2_sum+round3(gy2_adc.read()*3.3);
+      wait(0.005);
+    }
+    gy1_avg=round3(gy1_sum/20);
+    gy2_avg=round3(gy2_sum/20);
+    
+    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
+    gy2_wk1=round3((gy2_avg-gy2_center)*1000/5);
+    if(gy1_wk1 > 0){
+      gy1_wk1=gy1_wk1*1.07;// migi hosei
+    }
+    if(gy1_wk1 < 0){
+      gy1_wk1=gy1_wk1*1.0;//hidari hosei
+    }
+    
+    if(gy2_wk1 > 0){
+      gy2_wk1=gy2_wk1*1.0;// migi hosei
+    }
+    if(gy2_wk1 < 0){
+      gy2_wk1=gy2_wk1*1.1;//hidari hosei
+    }
+    
+    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
+    gy2_sgm=round3(gy2_sgm+(gy2_wk0+gy2_wk1)*0.1/2);
+    gy1_wk0=gy1_wk1;
+    gy2_wk0=gy2_wk1;
+    
+    if(gy1_sgm > 90){
+      gy1_sgm=90;  
+    }
+    else if(gy1_sgm < -90){
+      gy1_sgm=-90;
+    }
+    
+    if(gy2_sgm > 90){
+      gy2_sgm=90;  
+    }
+    else if(gy2_sgm < -90){
+      gy2_sgm=-90;
+    }
+
+    pwidth1=-gy1_sgm;
+    pwidth1=(pwidth1+90)/180/1000+0.001;
+    gy1_lc=(gy1_sgm-5)/10+9;
+    if(gy1_lc >20){
+      gy1_lc=19;
+    }
+    if(gy1_lc <0){
+      gy1_lc=0;
+    }
+    
+    pwidth2=gy2_sgm;
+    pwidth2=(pwidth2+90)/180/1000+0.001;
+    gy2_lc=(gy2_sgm-5)/10+9;
+    if(gy2_lc >20){
+      gy2_lc=19;
+    }
+    if(gy2_lc <0){
+      gy2_lc=0;
+    }
+    
+    lcd.locate(0,1);
+    //lcd.printf("%3.2f %3.2f %2.0f %2.1f",gy1_avg,gy1_center,gy1_sgm,pwidth*1000);
+    //lcd.locate(0,2);
+    lcd.printf("--------+--------");
+    lcd.locate(gy1_lc_old,2);
+    lcd.printf(" ");
+    lcd.locate(gy1_lc,2);
+    lcd.printf("*");
+    gy1_lc_old=gy1_lc;
+    servo1.pulsewidth(pwidth1);
+    lcd.locate(gy2_lc_old,3);
+    lcd.printf(" ");
+    lcd.locate(gy2_lc,3);
+    lcd.printf("*");
+    gy2_lc_old=gy2_lc;
+    servo2.pulsewidth(pwidth2);
+  }//while    
+}//main      
+