Takeuchi Kouichi / Mbed 2 deprecated 2bk0819_Gyro_test04

エレキジャックweb mbed入門 ジャイロとサーボを動かそう 課題4です。

Dependencies:   mbed

main.cpp@0:86acb35db6d7, 2011-09-20 (annotated)

Committer:
takeuchi
Date:
Tue Sep 20 09:56:24 2011 +0000
Revision:
0:86acb35db6d7

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


takeuchi

0:86acb35db6d7


     1

//Gyro test04





takeuchi

0:86acb35db6d7


     2

// Akidukidensi AE-GYRO-SMD





takeuchi

0:86acb35db6d7


     3

#include "mbed.h"





takeuchi

0:86acb35db6d7


     4

#include "TextLCD0420.h"





takeuchi

0:86acb35db6d7


     5







takeuchi

0:86acb35db6d7


     6

#define ON 1





takeuchi

0:86acb35db6d7


     7

#define OFF 0





takeuchi

0:86acb35db6d7


     8

#define Gy1_offset 0.01





takeuchi

0:86acb35db6d7


     9







takeuchi

0:86acb35db6d7


    10

DigitalOut mled0(LED1);





takeuchi

0:86acb35db6d7


    11

DigitalOut mled1(LED2);





takeuchi

0:86acb35db6d7


    12

AnalogIn gy1_adc(p16);





takeuchi

0:86acb35db6d7


    13

AnalogIn gy2_adc(p17);





takeuchi

0:86acb35db6d7


    14







takeuchi

0:86acb35db6d7


    15

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3





takeuchi

0:86acb35db6d7


    16







takeuchi

0:86acb35db6d7


    17

double round3(double x){





takeuchi

0:86acb35db6d7


    18

  double y;





takeuchi

0:86acb35db6d7


    19

  y=double(int((x+0.005)*100))/100;





takeuchi

0:86acb35db6d7


    20

  return(y);





takeuchi

0:86acb35db6d7


    21

}





takeuchi

0:86acb35db6d7


    22







takeuchi

0:86acb35db6d7


    23

int main() {





takeuchi

0:86acb35db6d7


    24

  float gy1_data;





takeuchi

0:86acb35db6d7


    25

  float gy1_center;





takeuchi

0:86acb35db6d7


    26

  float gy1_sgm=0;





takeuchi

0:86acb35db6d7


    27

  float gy1_wk0=0,gy1_wk1=0;





takeuchi

0:86acb35db6d7


    28

  float gy1_avg=0,gy1_sum;





takeuchi

0:86acb35db6d7


    29

  int i,gy1_lc=9,gy1_lc_old=0;





takeuchi

0:86acb35db6d7


    30

  





takeuchi

0:86acb35db6d7


    31

  lcd.cls();





takeuchi

0:86acb35db6d7


    32

  lcd.locate(0,0);





takeuchi

0:86acb35db6d7


    33

  lcd.printf("*** Gyro test04 ****\n");





takeuchi

0:86acb35db6d7


    34







takeuchi

0:86acb35db6d7


    35

  gy1_center=0;





takeuchi

0:86acb35db6d7


    36

  for(i=0;i<300;i++){





takeuchi

0:86acb35db6d7


    37

    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);





takeuchi

0:86acb35db6d7


    38

    wait(0.001);





takeuchi

0:86acb35db6d7


    39

  }





takeuchi

0:86acb35db6d7


    40

  gy1_center=round3(gy1_center/300);





takeuchi

0:86acb35db6d7


    41

  





takeuchi

0:86acb35db6d7


    42

  while(1){





takeuchi

0:86acb35db6d7


    43

    gy1_sum=0;





takeuchi

0:86acb35db6d7


    44

    for(i=0;i<20;i++){





takeuchi

0:86acb35db6d7


    45

      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);





takeuchi

0:86acb35db6d7


    46

      wait(0.005);





takeuchi

0:86acb35db6d7


    47

    }





takeuchi

0:86acb35db6d7


    48

    gy1_avg=round3(gy1_sum/20);





takeuchi

0:86acb35db6d7


    49

    





takeuchi

0:86acb35db6d7


    50

    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);





takeuchi

0:86acb35db6d7


    51

    if(gy1_wk1 < 0){





takeuchi

0:86acb35db6d7


    52

      gy1_wk1=gy1_wk1*1.1;





takeuchi

0:86acb35db6d7


    53

    }





takeuchi

0:86acb35db6d7


    54

    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);





takeuchi

0:86acb35db6d7


    55

    gy1_wk0=gy1_wk1;





takeuchi

0:86acb35db6d7


    56

    





takeuchi

0:86acb35db6d7


    57

    if(gy1_sgm > 90){





takeuchi

0:86acb35db6d7


    58

      gy1_sgm=90;  





takeuchi

0:86acb35db6d7


    59

    }





takeuchi

0:86acb35db6d7


    60

    else if(gy1_sgm < -90){





takeuchi

0:86acb35db6d7


    61

      gy1_sgm=-90;





takeuchi

0:86acb35db6d7


    62

    }





takeuchi

0:86acb35db6d7


    63

    





takeuchi

0:86acb35db6d7


    64

    lcd.locate(0,1);





takeuchi

0:86acb35db6d7


    65

    lcd.printf("%3.3f %3.3f d:%3.0f",gy1_avg,gy1_center,gy1_sgm);





takeuchi

0:86acb35db6d7


    66

    lcd.locate(0,2);





takeuchi

0:86acb35db6d7


    67

    lcd.printf("--------+--------");





takeuchi

0:86acb35db6d7


    68

    gy1_lc=(gy1_sgm-5)/10+9;





takeuchi

0:86acb35db6d7


    69

    if(gy1_lc >20){





takeuchi

0:86acb35db6d7


    70

      gy1_lc=19;





takeuchi

0:86acb35db6d7


    71

    }





takeuchi

0:86acb35db6d7


    72

    if(gy1_lc <0){





takeuchi

0:86acb35db6d7


    73

      gy1_lc=0;





takeuchi

0:86acb35db6d7


    74

    }





takeuchi

0:86acb35db6d7


    75

    lcd.locate(gy1_lc_old,3);





takeuchi

0:86acb35db6d7


    76

    lcd.printf(" ");





takeuchi

0:86acb35db6d7


    77

    lcd.locate(gy1_lc,3);





takeuchi

0:86acb35db6d7


    78

    lcd.printf("*");





takeuchi

0:86acb35db6d7


    79

    gy1_lc_old=gy1_lc;





takeuchi

0:86acb35db6d7


    80

  }//while    





takeuchi

0:86acb35db6d7


    81

}//main      





takeuchi

0:86acb35db6d7


    82