main.cpp

   1 /* main.cpp for TOPPERS/ATK(OSEK) */ \r
   2 \r
   3 // ECRobot++ API\r
   4 #include "Lcd.h"\r
   5 #include "Bluetooth.h"\r
   6 #include "Clock.h"\r
   7 #include "GyroSensor.h"\r
   8 #include "LightSensor.h"\r
   9 #include "Motor.h"\r
  10 #include "Nxt.h"\r
  11 #include "SonarSensor.h"\r
  12 #include "Speaker.h"\r
  13 #include "TouchSensor.h"\r
  14 \r
  15 #include "tsprintf.h"\r
  16 #include "ETLineTracer.h"\r
  17 #include "ETLinePos.h"\r
  18 #include "ETBalanceRunner.h"\r
  19 #include "ETTailControl.h"\r
  20 #include "context.h"\r
  21 \r
  22 using namespace ecrobot;\r
  23 \r
  24 extern "C"\r
  25 {\r
  26 #include "kernel.h"\r
  27 #include "kernel_id.h"\r
  28 #include "ecrobot_interface.h"\r
  29 \r
  30 DeclareTask(TaskMain);\r
  31 DeclareCounter(SysTimerCnt);//Alarm\97\98\97p\8e\9e\95K\97v\r
  32 \r
  33 // global object instances\r
  34 Nxt nxt;\r
  35 Lcd lcd;\r
  36 Speaker speaker;\r
  37 Bluetooth bt;\r
  38 Clock clock;\r
  39 GyroSensor gyro(PORT_1);\r
  40 SonarSensor sonar(PORT_2);\r
  41 LightSensor light(PORT_3);\r
  42 TouchSensor touch(PORT_4);\r
  43 Motor motorT(PORT_A);\r
  44 Motor motorR(PORT_B);\r
  45 Motor motorL(PORT_C);\r
  46 \r
  47 ETBalanceRunner runner(motorL,motorR,gyro,nxt);\r
  48 ETLinePos linepos(light);\r
  49 ETLineTracer tracer(linepos,runner);\r
  50 ETTailControl tailcontroler(motorT,0);\r
  51 ContextStuff::Context context;\r
  52  \r
  53 // nxtOSEK hook to be invoked from an ISR in category 2\r
  54 void user_1ms_isr_type2(void)\r
  55 {\r
  56     //  //Sleep\97\98\97p\8e\9e\95K\97v\r
  57     SleeperMonitor();\r
  58     //  //Alarm\97\98\97p\8e\9e\95K\97v\r
  59     StatusType ercd;\r
  60     ercd = SignalCounter(SysTimerCnt);\r
  61     if (ercd != E_OK) {\r
  62         ShutdownOS(ercd);\r
  63     }\r
  64 }\r
  65 \r
  66 TASK(TaskMain)\r
  67 {\r
  68         lcd.clear();\r
  69         lcd.putf("s", "Hello World");\r
  70         lcd.disp();\r
  71     tailcontroler.Init();\r
  72     tailcontroler.SetAngle(ETTailControl::ANGLE_STAND_UP);\r
  73 \r
  74 \r
  75         // TODO:\82Æ\82è\82 \82¦\82¸\82Ì\83L\83\83\83\8a\83u\83\8c\81[\83V\83\87\83\93\r
  76         static const int sum_num = 256;\r
  77         unsigned long sum = 0;\r
  78         char str[16+1];\r
  79         // \83W\83\83\83C\83\8d\r
  80         while(!touch.isPressed()){\r
  81         tailcontroler.Control();\r
  82         }\r
  83         while(touch.isPressed()){\r
  84         }\r
  85         for(int i=0;i<sum_num;i++){\r
  86                 sum += gyro.get();\r
  87                 clock.wait(10);\r
  88         }\r
  89         unsigned long offset = sum / sum_num;\r
  90         runner.SetGyroOffset(offset);\r
  91         tsprintf(str,"Offset = %d",offset);\r
  92         lcd.cursor(0,1);\r
  93         lcd.putf("s",str);\r
  94         lcd.disp();\r
  95         speaker.playTone(1000,100,50);\r
  96 \r
  97         // \94\92\r
  98         while(!touch.isPressed()){\r
  99         tailcontroler.Control();\r
 100         }\r
 101         while(touch.isPressed()){\r
 102         }\r
 103         sum = 0;\r
 104         for(int i=0;i<sum_num;i++){\r
 105                 sum += light.getBrightness();\r
 106                 clock.wait(10);\r
 107         }\r
 108         unsigned long white = sum / sum_num;\r
 109         linepos.SetWhite(white);\r
 110         tsprintf(str,"White = %d",white);\r
 111         lcd.cursor(0,2);\r
 112         lcd.putf("s",str);\r
 113         lcd.disp();\r
 114         speaker.playTone(1200,100,50);\r
 115 \r
 116         // \8d\95\r
 117         while(!touch.isPressed()){\r
 118         tailcontroler.Control();\r
 119         }\r
 120         while(touch.isPressed()){\r
 121         }\r
 122         sum = 0;\r
 123         for(int i=0;i<sum_num;i++){\r
 124                 sum += light.getBrightness();\r
 125                 clock.wait(10);\r
 126         }\r
 127         unsigned long black = sum / sum_num;\r
 128         linepos.SetBlack(black);\r
 129         tsprintf(str,"Black = %d",black);\r
 130         lcd.cursor(0,3);\r
 131         lcd.putf("s",str);\r
 132         lcd.disp();\r
 133         speaker.playTone(1400,100,50);\r
 134 \r
 135         while(!touch.isPressed()){\r
 136         tailcontroler.Control();\r
 137         }\r
 138         while(touch.isPressed()){\r
 139         }\r
 140         tailcontroler.SetAngle(ETTailControl::ANGLE_DRIVE);\r
 141     context.SetETLineTracer(&tracer);\r
 142         TerminateTask();\r
 143 }\r
 144 \r
 145 TASK(Task4ms)\r
 146 {\r
 147         if(touch.isPressed()){\r
 148 //              motorR.setPWM(0);\r
 149 //              motorL.setPWM(0);\r
 150                 ActivateTask(TaskMain);\r
 151                 TerminateTask();\r
 152         }\r
 153     context.Display(lcd);\r
 154     context.CyclicExcute();\r
 155     tailcontroler.Control();\r
 156 \r
 157         TerminateTask();\r
 158 }\r
 159 \r
 160 int __cxa_pure_virtual(){\r
 161         // \8f\83\90\88\89¼\91z\8aÖ\90\94\82ª\83I\81[\83o\81[\83\89\83C\83h\82µ\82È\82¢\82Å\8cÄ\82Ñ\8fo\82³\82ê\82½\8e\9e\82Ì\83G\83\89\81[\83n\83\93\83h\83\89\82ð\8bó\8eÀ\91\95\r
 162         // \82±\82ê\82ð\93ü\82ê\82È\82¢\82Æram\8eg\97p\97Ê\82ª\82Æ\82Ä\82à\91\9d\82¦\82é\81E\81E\81E\r
 163         return 0;\r
 164 }\r
 165 \r
 166 }       // extern "C"\r