Takeuchi Kouichi / Mbed 2 deprecated 2bk1209_TAgyro03

エレキジャックweb mbed入門 多摩川精機ジャイロTAG201 課題3です。ジャイロから読み込んだ角速度を積分して角度を表示します。

Dependencies:   mbed

main.cpp

Committer:
takeuchi
Date:
2011-12-09
Revision:
0:401cde3ddd78

File content as of revision 0:401cde3ddd78:

// TAGyro03
// Tamagawaseiki GYRO TAG201

#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define XON 0
#define XOFF 1

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc_in(p16);
 DigitalIn TSW(p15);

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);
}

double xabs(double x,double y){
  if(x>=y){
    return(x-y);
  }
  else if(y>x){
    return(y-x);
  }
}

int main() {
  float gy1_center,gy1_data,gy1_adc,gy1_sum;
  float gy1_wk0=0,gy1_wk1=0,gy1_kakudo=0;
  int i;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** TA-Gyro03 ***");
  
  gy1_sum=0;
  for(i=0;i<10;i++){
      gy1_sum=gy1_sum+round3(gy1_adc_in.read()*5.0);
      lcd.locate(0,1);
      lcd.printf("Gyro init:%3d",10-i);
      wait(0.1);
  }
  
  gy1_center=(gy1_sum/10);
  
  while(1){
        
    gy1_adc=(gy1_adc_in.read()*5.0);// Gy1 read
    wait(0.08);
    if(xabs(gy1_adc,gy1_center) <=0.07){ 
      gy1_data=0;     
    } 
    else{
      gy1_data=gy1_adc-gy1_center;   
    }

    gy1_wk1=(gy1_data*1000/6.0);// sekibun jyunbi

    //sekibun 
    gy1_kakudo=round3(gy1_kakudo+(gy1_wk0+gy1_wk1)*0.08/2);
    gy1_wk0=gy1_wk1;    
    
    if(TSW==XON){// center reset
      gy1_kakudo=0;
    }        
   
    lcd.locate(0,1);
    lcd.printf("D:%3.2f C:%3.2f %2.0fd ",gy1_data,gy1_center,gy1_kakudo);
   }//while    
}//main