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 "BTConnection.h"\r
   7 #include "Clock.h"\r
   8 #include "GyroSensor.h"\r
   9 #include "LightSensor.h"\r
  10 #include "Motor.h"\r
  11 #include "Nxt.h"\r
  12 #include "SonarSensor.h"\r
  13 #include "Speaker.h"\r
  14 #include "TouchSensor.h"\r
  15 \r
  16 #include "tsprintf.h"\r
  17 #include "ETLineTracer.h"\r
  18 #include "ETLinePos.h"\r
  19 #include "ETBalanceRunner.h"\r
  20 #include "ETTailControl.h"\r
  21 #include "context.h"\r
  22 \r
  23 using namespace ecrobot;\r
  24 \r
  25 /* sample_c4\83}\83N\83\8d */\r
  26 #define DEVICE_NAME       "ET277"  /* Bluetooth\92Ê\90M\97p\83f\83o\83C\83X\96¼ */                                      /* \83`\81[\83\80ID\82É\95Ï\8dX\82µ\82Ä\82\82¾\82³\82¢ */\r
  27 #define PASS_KEY          "1234" /* Bluetooth\92Ê\90M\97p\83p\83X\83L\81[ */                                          /* \8cÅ\97L\82Ì\83p\83X\83L\81[\82ð\90Ý\92è\82µ\82Ä\82\82¾\82³\82¢ */\r
  28 #define CMD_START         '1'    /* \83\8a\83\82\81[\83g\83X\83^\81[\83g\83R\83}\83\93\83h */\r
  29 #define CMD_STOP          '9'    /* \83\8a\83\82\81[\83g\83X\83g\83b\83v\83R\83}\83\93\83h */\r
  30 \r
  31 \r
  32 extern "C"\r
  33 {\r
  34 #include "kernel.h"\r
  35 #include "kernel_id.h"\r
  36 #include "ecrobot_interface.h"\r
  37 \r
  38 DeclareTask(TaskMain);\r
  39 DeclareCounter(SysTimerCnt);//Alarm\97\98\97p\8e\9e\95K\97v\r
  40 \r
  41 // global object instances\r
  42 Nxt nxt;\r
  43 Lcd lcd;\r
  44 Speaker speaker;\r
  45 Bluetooth bt;\r
  46 Clock clock;\r
  47 GyroSensor gyro(PORT_1);\r
  48 SonarSensor sonar(PORT_2);\r
  49 LightSensor light(PORT_3);\r
  50 TouchSensor touch(PORT_4);\r
  51 Motor motorT(PORT_A);\r
  52 Motor motorR(PORT_B);\r
  53 Motor motorL(PORT_C);\r
  54 \r
  55 ETBalanceRunner runner(motorL,motorR,gyro,nxt);\r
  56 ETLinePos linepos(light);\r
  57 ETLineTracer tracer(linepos,runner);\r
  58 ETTailControl tailcontroler(motorT,0);\r
  59 ContextStuff::Context context;\r
  60 \r
  61 /* \83f\83o\83C\83X\8fI\97¹\97p\83t\83b\83N\8aÖ\90\94 */\r
  62 /* \82±\82Ì\8aÖ\90\94\82ÍSTOP\82Ü\82½\82ÍEXIT\83{\83^\83\93\82ª\89\9f\82³\82ê\82½\8e\9e\82É\8eÀ\8ds\82³\82ê\82Ü\82·\81B */\r
  63 void ecrobot_device_terminate()\r
  64 {\r
  65         /* \83Z\83\93\83T\81A\83\82\81[\83^\82È\82Ç\82Ì\8ae\83f\83o\83C\83X\82Ì\8fI\97¹\8aÖ\90\94\82ð\r
  66          * \82±\82±\82Å\8eÀ\91\95\82·\82é\82±\82Æ\82ª\82Å\82«\82Ü\82·\81B\r
  67          * \81Ë\81@\8cõ\83Z\83\93\83T\90Ô\90FLED\82ðOFF\82É\82·\82é\r
  68          */\r
  69         //ecrobot_set_light_sensor_inactive(SENSOR_LIGHT);\r
  70         ecrobot_term_bt_connection(); /* Bluetooth\92Ê\90M\82ð\8fI\97¹ */                                                 //ex4\r
  71 }\r
  72 \r
  73  \r
  74 // nxtOSEK hook to be invoked from an ISR in category 2\r
  75 void user_1ms_isr_type2(void)\r
  76 {\r
  77     //  //Sleep\97\98\97p\8e\9e\95K\97v\r
  78     SleeperMonitor();\r
  79     //  //Alarm\97\98\97p\8e\9e\95K\97v\r
  80     StatusType ercd;\r
  81     ercd = SignalCounter(SysTimerCnt);\r
  82     if (ercd != E_OK) {\r
  83         ShutdownOS(ercd);\r
  84     }\r
  85 }\r
  86 \r
  87 /* Bluetooth\92Ê\90M\97p\83f\81[\83^\8eó\90M\83o\83b\83t\83@ */\r
  88 static char rx_buf[BT_MAX_RX_BUF_SIZE];\r
  89 \r
  90 /*****************************************************************************\r
  91  \8aÖ\90\94\96¼ : Bluetooth_isReceived\r
  92  \88ø  \90\94 : \8eó\90M\91Ò\82¿\83R\83}\83\93\83h\r
  93  \95Ô\82è\92l : 1(\83X\83^\81[\83g)/0(\91Ò\8b@)\r
  94  \8aT  \97v : Bluetooth\92Ê\90M\82É\82æ\82é\83\8a\83\82\81[\83g\83X\83^\81[\83g\81B Tera Term\82È\82Ç\82Ì\83^\81[\83~\83i\83\8b\83\\83t\83g\82©\82ç\81A\r
  95           ASCII\83R\81[\83h\82Å1\82ð\91\97\90M\82·\82é\82Æ\81A\83\8a\83\82\81[\83g\83X\83^\81[\83g\82·\82é\81B\r
  96  *****************************************************************************/\r
  97 BOOL Bluetooth_isReceived(char cmd)\r
  98 {\r
  99         int i;\r
 100         unsigned int rx_len;\r
 101         unsigned char come = 0;\r
 102 \r
 103         for (i=0; i<BT_MAX_RX_BUF_SIZE; i++)\r
 104         {\r
 105                 rx_buf[i] = 0; /* \8eó\90M\83o\83b\83t\83@\82ð\83N\83\8a\83A */\r
 106         }\r
 107 \r
 108         rx_len = ecrobot_read_bt(rx_buf, 0, BT_MAX_RX_BUF_SIZE);\r
 109         if (rx_len > 0)\r
 110         {\r
 111                 /* \8eó\90M\83f\81[\83^\82 \82è */\r
 112                 if (rx_buf[0] == cmd)\r
 113                 {\r
 114                         come = 1; /* \83R\83}\83\93\83h\8eó\90M */\r
 115                 }\r
 116                 ecrobot_send_bt(rx_buf, 0, rx_len); /* \8eó\90M\83f\81[\83^\82ð\83G\83R\81[\83o\83b\83N */\r
 117         }\r
 118 \r
 119         return (BOOL)come;\r
 120 }\r
 121 \r
 122 bool is_touched(TouchSensor& touch,ETTailControl& tailcontroler)\r
 123 {\r
 124     tailcontroler.Control();\r
 125     if(touch.isPressed()){\r
 126             systick_wait_ms(30);\r
 127         while(touch.isPressed()){\r
 128             tailcontroler.Control();\r
 129         }\r
 130             systick_wait_ms(30);\r
 131         return true;\r
 132     }\r
 133     return false;\r
 134 }\r
 135 \r
 136 \r
 137 void waitstart(TouchSensor& touch,ETTailControl& tailcontroler,char cmd)\r
 138 {\r
 139     /* \83^\83b\83`\83Z\83\93\83T\82ª\89\9f\82³\82ê\82é\82Ü\82Å\81A\91Ò\82¿\91±\82¯\82é */\r
 140     while(1){\r
 141             if (is_touched(touch,tailcontroler)){\r
 142                     break;\r
 143             }\r
 144             if (Bluetooth_isReceived(CMD_START)){                                                                       \r
 145                     break;\r
 146             }\r
 147     }\r
 148 }\r
 149 \r
 150 TASK(TaskMain)\r
 151 {\r
 152     //bt.setFriendlyName(DEVICE_NAME);\r
 153         BTConnection btConnection(bt, lcd, nxt);\r
 154         btConnection.connect(PASS_KEY);\r
 155         lcd.clear();\r
 156         lcd.putf("s", "Hello World");\r
 157         lcd.disp();\r
 158     tailcontroler.Init();\r
 159     tailcontroler.SetAngle(ETTailControl::ANGLE_STAND_UP);\r
 160 \r
 161 \r
 162         // TODO:\82Æ\82è\82 \82¦\82¸\82Ì\83L\83\83\83\8a\83u\83\8c\81[\83V\83\87\83\93\r
 163         static const int sum_num = 256;\r
 164         unsigned long sum = 0;\r
 165         char str[16+1];\r
 166         // \83W\83\83\83C\83\8d\r
 167         while(!is_touched(touch,tailcontroler)) {}\r
 168         for(int i=0;i<sum_num;i++){\r
 169                 sum += gyro.get();\r
 170                 clock.wait(10);\r
 171         }\r
 172         unsigned long offset = sum / sum_num;\r
 173         runner.SetGyroOffset(offset);\r
 174         tsprintf(str,"Offset = %d",offset);\r
 175         lcd.cursor(0,1);\r
 176         lcd.putf("s",str);\r
 177         lcd.disp();\r
 178         speaker.playTone(1000,100,50);\r
 179 \r
 180         // \94\92\r
 181         while(!is_touched(touch,tailcontroler)) {}\r
 182         sum = 0;\r
 183         for(int i=0;i<sum_num;i++){\r
 184                 sum += light.getBrightness();\r
 185                 clock.wait(10);\r
 186         }\r
 187         unsigned long white = sum / sum_num;\r
 188         linepos.SetWhite(white);\r
 189         tsprintf(str,"White = %d",white);\r
 190         lcd.cursor(0,2);\r
 191         lcd.putf("s",str);\r
 192         lcd.disp();\r
 193         speaker.playTone(1200,100,50);\r
 194 \r
 195         // \8d\95\r
 196         while(!is_touched(touch,tailcontroler)) {}\r
 197         sum = 0;\r
 198         for(int i=0;i<sum_num;i++){\r
 199                 sum += light.getBrightness();\r
 200                 clock.wait(10);\r
 201         }\r
 202         unsigned long black = sum / sum_num;\r
 203         linepos.SetBlack(black);\r
 204         tsprintf(str,"Black = %d",black);\r
 205         lcd.cursor(0,3);\r
 206         lcd.putf("s",str);\r
 207         lcd.disp();\r
 208         speaker.playTone(1400,100,50);\r
 209 \r
 210     //wait for touch or bluetooth.\r
 211     waitstart(touch,tailcontroler,CMD_START);\r
 212 \r
 213         tailcontroler.SetAngle(ETTailControl::ANGLE_DRIVE);\r
 214     context.SetETLineTracer(&tracer);\r
 215         TerminateTask();\r
 216 }\r
 217 \r
 218 TASK(Task4ms)\r
 219 {\r
 220         if(touch.isPressed()){\r
 221 //              motorR.setPWM(0);\r
 222 //              motorL.setPWM(0);\r
 223                 tracer.Stop();\r
 224                 tracer.Reset();\r
 225                 ActivateTask(TaskMain);\r
 226                 TerminateTask();\r
 227                 return;\r
 228         }\r
 229     context.Display(lcd);\r
 230     context.CyclicExcute();\r
 231     tailcontroler.Control();\r
 232 \r
 233         TerminateTask();\r
 234 }\r
 235 \r
 236 int __cxa_pure_virtual(){\r
 237         // \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
 238         // \82±\82ê\82ð\93ü\82ê\82È\82¢\82Æram\8eg\97p\97Ê\82ª\82Æ\82Ä\82à\91\9d\82¦\82é\81E\81E\81E\r
 239         return 0;\r
 240 }\r
 241 \r
 242 }       // extern "C"\r