tokai_ex4のようなキャリブレーションを適当に実装してライントレースできるようにした。

diff --git a/ETBalanceRunner.cpp b/ETBalanceRunner.cpp
index 1f73778..835ad28 100644 (file)
--- a/ETBalanceRunner.cpp
+++ b/ETBalanceRunner.cpp
@@ -12,6 +12,7 @@ namespace ecrobot{
 ETBalanceRunner::ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt)\r
        : m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610)\r
 {\r
+       balance_init();\r
 }\r
 ETBalanceRunner::~ETBalanceRunner(void)\r
 {\r
@@ -19,7 +20,7 @@ ETBalanceRunner::~ETBalanceRunner(void)
 void ETBalanceRunner::Run(int forward, int turn)\r
 {\r
        S8 r,l;\r
-       balance_control(forward,turn,m_gyro.getAnglerVelocity(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);\r
+       balance_control(forward,turn,m_gyro.get(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);\r
        m_motorL.setPWM(l);\r
        m_motorR.setPWM(r);\r
 }\r

diff --git a/main.cpp b/main.cpp
index e0ea1b3..e979f1c 100644 (file)
--- a/main.cpp
+++ b/main.cpp
@@ -64,6 +64,68 @@ TASK(TaskMain)
        lcd.putf("s", "Hello World");\r
        lcd.disp();\r
 \r
+       // TODO:\82Æ\82è\82 \82¦\82¸\82Ì\83L\83\83\83\8a\83u\83\8c\81[\83V\83\87\83\93\r
+       static const int sum_num = 256;\r
+       unsigned long sum = 0;\r
+       char str[16+1];\r
+       // \83W\83\83\83C\83\8d\r
+       while(!touch.isPressed()){\r
+       }\r
+       while(touch.isPressed()){\r
+       }\r
+       for(int i=0;i<sum_num;i++){\r
+               sum += gyro.get();\r
+               clock.wait(10);\r
+       }\r
+       unsigned long offset = sum / sum_num;\r
+       runner.SetGyroOffset(offset);\r
+       tsprintf(str,"Offset = %d",offset);\r
+       lcd.cursor(0,1);\r
+       lcd.putf("s",str);\r
+       lcd.disp();\r
+       speaker.playTone(1000,100,50);\r
+\r
+       // \94\92\r
+       while(!touch.isPressed()){\r
+       }\r
+       while(touch.isPressed()){\r
+       }\r
+       sum = 0;\r
+       for(int i=0;i<sum_num;i++){\r
+               sum += light.getBrightness();\r
+               clock.wait(10);\r
+       }\r
+       unsigned long white = sum / sum_num;\r
+       linepos.SetWhite(white);\r
+       tsprintf(str,"White = %d",white);\r
+       lcd.cursor(0,2);\r
+       lcd.putf("s",str);\r
+       lcd.disp();\r
+       speaker.playTone(1200,100,50);\r
+\r
+       // \8d\95\r
+       while(!touch.isPressed()){\r
+       }\r
+       while(touch.isPressed()){\r
+       }\r
+       sum = 0;\r
+       for(int i=0;i<sum_num;i++){\r
+               sum += light.getBrightness();\r
+               clock.wait(10);\r
+       }\r
+       unsigned long black = sum / sum_num;\r
+       linepos.SetBlack(black);\r
+       tsprintf(str,"Black = %d",black);\r
+       lcd.cursor(0,3);\r
+       lcd.putf("s",str);\r
+       lcd.disp();\r
+       speaker.playTone(1400,100,50);\r
+\r
+       while(!touch.isPressed()){\r
+       }\r
+       while(touch.isPressed()){\r
+       }\r
+\r
        TerminateTask();\r
 }\r
 \r