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