main.cpp

00001 #include "mbed.h"
00002 #include "PololuLedStrip.h"
00003 #include <math.h>
00004 
00005 Serial Xbee(P0_10,P0_11,19200); //TX,RX,クロックレート
00006 Serial Xbee2(P0_0,P0_1,19200); //ＬＥＤに送信
00007 Serial pc(USBTX,USBRX); //pc(USBTX,USBRX)
00008 Ticker ticker;
00009 unsigned char RX_data;
00010 
00011 #define DIGITS 8
00012 #define ledsize 65
00013 //円周率
00014 #define PI 3.14159265358979
00015 //モータードライバ
00016 #define M1 0x01
00017 
00018 //モータードライバへの命令
00019 #define STOP 0
00020 #define CW 1
00021 #define CCW -1
00022 #define bSTOP 10
00023 #define bCW 11
00024 #define bCCW -11
00025 
00026 #define sCW 21
00027 #define sCCW -21
00028 #define sSTOP 20
00029 #define shCW 31
00030 #define shCCW -31
00031 
00032 #define lCW 51
00033 #define lCCW -51
00034 #define lSTOP 50
00035 #define lsCW 61
00036 #define lsCCW -61
00037 #define lsSTOP 60
00038 
00039 #define rON 41
00040 #define rOFF -41
00041 
00042 DigitalOut led1(LED1);
00043 DigitalOut led2(LED2);
00044 DigitalOut led3(LED3);
00045 DigitalOut ledx(LED4);
00046 I2C md_i2c_tx(P0_27, P0_28);
00047 
00048 void cv2bin( unsigned char n ) {
00049     int i;
00050 
00051     for( i = DIGITS-1; i >= 0; i-- ) {
00052         printf( "%d", ( n >> i ) & 1 );
00053     }
00054     printf( "\n" );
00055 }
00056 
00057 //i2c通信
00058 void md_setoutput(unsigned char address, int rotate, int duty) {
00059     char val;
00060     char data[2];
00061         /* H30新型：処理最適化 */
00062         switch (rotate) {
00063             case CW:    data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
00064             case CCW:   data[0] = 'R'; break;
00065             case STOP:  data[0] = 'S'; break;
00066 
00067             case bCW:   data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ） //ブレーキ停止は機体に負荷がかかるため使用禁止
00068             case bCCW:  data[0] = 'r'; break;
00069             case bSTOP: data[0] = 's'; break;
00070             
00071             case sCW:   data[0] = 'A'; break; // 速度比例制御モード
00072             case sCCW:  data[0] = 'B'; break;
00073             case sSTOP: data[0] = 'T'; break;
00074             
00075             case shCW:   data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
00076             case shCCW:  data[0] = 'b'; break;
00077 
00078             case lCW:   data[0] = 'L'; break; // 通常LAP
00079             case lCCW:  data[0] = 'M'; break;
00080             case lSTOP: data[0] = 'N'; break;
00081             case lsCW:   data[0] = 'l'; break; // 速調LAP
00082             case lsCCW:  data[0] = 'm'; break;
00083             case lsSTOP: data[0] = 'n'; break;
00084 
00085             case rON: data[0] = 'P'; break; //リレー駆動モード
00086             case rOFF: data[0] = 'p'; break;
00087         }
00088             
00089     data[1] = duty & 0xFF;
00090     
00091     val = md_i2c_tx.write(address << 1, data, 2); //duty送信
00092 }
00093 #define LED_COUNT 66 //LEDの数
00094 PololuLedStrip ledStrip1(P2_13);
00095 rgb_color colors[LED_COUNT];
00096 int cnt = 0;
00097 int EMS = 0;
00098 int con = 0;
00099 int motorOn = 0;
00100 int LEDDATA = 1;
00101 
00102 void Xbee_RX(){
00103     
00104     RX_data = Xbee.getc(); //Xbeeから送られてきた文字を代入
00105     pc.putc(RX_data); //値をpcに表示
00106     
00107     //非常停止　EMSが１で入る
00108     if (cnt == 0 && RX_data == 'S'){
00109         cnt++;
00110     }else if (cnt == 1 && RX_data == 'T'){
00111         cnt++;
00112     }else if (cnt == 2 && RX_data == 'O'){
00113         cnt++;
00114     }else if (cnt == 3 && RX_data == 'P'){
00115         cnt = 0;
00116         EMS = 1;
00117         led1 = 1;
00118     }else if (RX_data == 'L'){
00119         cnt = 0;
00120         EMS = 0;
00121         led1 = 0;
00122     }
00123 }
00124 
00125 rgb_color COLOR(float H,int n){
00126 
00127     float r = 0, g = 0, b = 0;
00128     H = 100; //光の強さ
00129     switch (n){
00130         case 1:r = H; g = 0; b = 0; break; //赤
00131         case 2:r = 0; g = H; b = 0; break; //緑
00132         case 3:r = 0; g = 0; b = H; break; //青
00133         case 4:r = H; g = H; b = 0; break; //黄色
00134         case 5:r = 0; g = H; b = H; break; //水色
00135         case 6:r = H; g = 0; b = H; break; //紫
00136         case 7:r = H; g = H; b = H; break; //白
00137         case 8:r = H; g = H * 0.5; b = 0; break; //オレンジ
00138         }
00139     return (rgb_color){r,g,b};
00140 }
00141 
00142 //フォトリフレクタ
00143 double fot;
00144 int h = 0;
00145 int b = 0;
00146 //↓LED色
00147 int R = 1;
00148 int G = 2;
00149 int B = 3;
00150 int Y = 4;
00151 int L = 5;
00152 int P = 6;
00153 int W = 7;
00154 int O = 8;
00155 
00156 void ledsw(){
00157     h++;
00158 }
00159 
00160 int main() {
00161     wait_ms(10);
00162     led1 = 1;
00163     led2 = 0;
00164     led3 = 0;
00165     ledx = 1;
00166     Xbee.attach(&Xbee_RX, Serial::RxIrq); //割り込み
00167     pc.baud(115200); //pcクロックレート
00168     md_i2c_tx.frequency(20000000UL/(16 + 2 * 2 * 16));//I2C通信
00169     
00170     while(1){
00171         //機構用モータ
00172         if(EMS){
00173             md_setoutput(M1,STOP,0);
00174         }else if(EMS == 0){
00175             md_setoutput(M1,CW,255);
00176         }
00177         
00178         //***************打ち上げ処理******************//
00179         if(RX_data == 'A' && con == 0){
00180             con = 1;
00181             h = 0;
00182             ticker.attach(ledsw,0.05);
00183         }else if(RX_data == 'B' && con == 1){
00184             con = 2;
00185             h = 0;
00186             ticker.attach(ledsw,0.2);
00187         }else if(RX_data == 'C' && con == 2){
00188             con = 3;
00189             h = 0;
00190             ticker.attach(ledsw,0.2);
00191         }else if(RX_data == 'D' && con == 3){
00192             con = 4;
00193             h = 0;
00194             ticker.attach(ledsw,0.2);
00195         }else if(RX_data == 'E' && con == 4){
00196             con = 5;
00197             h = 0;
00198             ticker.attach(ledsw,0.2);
00199         }else if(RX_data == 'A' && con == 5){
00200             con = 6;
00201             h = 0;
00202             ticker.attach(ledsw,0.2);
00203         }else if(RX_data == 'B' && con == 6){
00204             con = 7;
00205             h = 0;
00206             ticker.attach(ledsw,0.2);
00207         }else if(RX_data == 'C' && con == 7){
00208             con = 8;
00209             h = 0;
00210             ticker.attach(ledsw,0.2);
00211         }else if(RX_data == 'D' && con == 8){
00212             con = 9;
00213             h = 0;
00214             ticker.attach(ledsw,0.2);
00215         }
00216         
00217         if(con == 1){
00218             if(h < LED_COUNT + 1){
00219                 for(int k = 0; k < LED_COUNT; k++){
00220                     uint8_t phase =(k << 1);
00221                     colors[k] = COLOR(phase, 0);
00222                 }
00223                     
00224                 uint8_t phase =(h << 1);
00225                 colors[ledsize - h] = COLOR(phase, B);
00226                 ledStrip1.write(colors, LED_COUNT);
00227                  
00228                 if(h == LED_COUNT + 1){
00229                     ticker.detach();
00230                     Xbee2.putc('A');
00231                 }
00232             }
00233         }
00234         
00235         if(con == 2){
00236             if(h < LED_COUNT + 1){
00237                 for(int k = 0; k < LED_COUNT; k++){
00238                     uint8_t phase =(k << 1);
00239                     colors[k] = COLOR(phase, 0);
00240                 }
00241                     
00242                 uint8_t phase =(h << 1);
00243                 colors[ledsize - h] = COLOR(phase, G);
00244                 ledStrip1.write(colors, LED_COUNT);
00245                  
00246                 if(h == LED_COUNT + 1){
00247                     ticker.detach();
00248                     Xbee2.putc('B');
00249                 }
00250             }
00251         }
00252         
00253         if(con == 3){
00254             if(h < LED_COUNT + 1){
00255                 for(int k = 0; k < LED_COUNT; k++){
00256                     uint8_t phase =(k << 1);
00257                     colors[k] = COLOR(phase, 0);
00258                 }
00259                     
00260                 uint8_t phase =(h << 1);
00261                 colors[ledsize - h] = COLOR(phase, R);
00262                 ledStrip1.write(colors, LED_COUNT);
00263                   
00264                 if(h == LED_COUNT + 1){
00265                     ticker.detach();
00266                     Xbee2.putc('C');
00267                 }
00268             }
00269         }
00270         
00271         if(con == 4){
00272             if(h < LED_COUNT + 1){
00273                 for(int k = 0; k < LED_COUNT; k++){
00274                     uint8_t phase =(k << 1);
00275                     colors[k] = COLOR(phase, 0);
00276                 }
00277                     
00278                 uint8_t phase =(h << 1);
00279                 colors[ledsize - h] = COLOR(phase, O);
00280                 ledStrip1.write(colors, LED_COUNT);
00281                 
00282                 if(h == LED_COUNT + 1){
00283                     ticker.detach();
00284                     Xbee2.putc('D');
00285                 }
00286             }
00287         }
00288         
00289         if(con == 5){
00290             if(h < LED_COUNT + 1){
00291                 for(int k = 0; k < LED_COUNT; k++){
00292                     uint8_t phase =(k << 1);
00293                     colors[k] = COLOR(phase, 0);
00294                 }
00295                     
00296                 uint8_t phase =(h << 1);
00297                 colors[ledsize - h] = COLOR(phase, P);
00298                 ledStrip1.write(colors, LED_COUNT);
00299                 
00300                 if(h == LED_COUNT + 1){
00301                     ticker.detach();
00302                     Xbee2.putc('E');
00303                 }
00304             }
00305         }
00306         
00307         if(con == 6){
00308             if(h < LED_COUNT + 1){
00309                 for(int k = 0; k < LED_COUNT; k++){
00310                     uint8_t phase =(k << 1);
00311                     colors[k] = COLOR(phase, 0);
00312                 }
00313                     
00314                 uint8_t phase =(h << 1);
00315                 colors[ledsize - h] = COLOR(phase, P);
00316                 ledStrip1.write(colors, LED_COUNT);
00317                   
00318                 if(h == LED_COUNT + 1){
00319                     ticker.detach();
00320                     Xbee2.putc('F');
00321                 }
00322             }
00323         }
00324         
00325         if(con == 7){
00326             if(h < LED_COUNT + 1){
00327                 for(int k = 0; k < LED_COUNT; k++){
00328                     uint8_t phase =(k << 1);
00329                     colors[k] = COLOR(phase, 0);
00330                 }
00331                     
00332                 uint8_t phase =(h << 1);
00333                 colors[ledsize - h] = COLOR(phase, P);
00334                 ledStrip1.write(colors, LED_COUNT);
00335                 
00336                     
00337                 if(h == LED_COUNT + 1){
00338                     ticker.detach();
00339                     Xbee2.putc('G');
00340                 }
00341             }
00342         }
00343         
00344         if(con == 8){
00345             if(h < LED_COUNT + 1){
00346                 for(int k = 0; k < LED_COUNT; k++){
00347                     uint8_t phase =(k << 1);
00348                     colors[k] = COLOR(phase, 0);
00349                 }
00350                     
00351                 uint8_t phase =(h << 1);
00352                 colors[ledsize - h] = COLOR(phase, P);
00353                 ledStrip1.write(colors, LED_COUNT);
00354                 
00355                    
00356                 if(h == LED_COUNT + 1){
00357                     ticker.detach();
00358                     Xbee2.putc('H');
00359                 }
00360             }
00361         }
00362         
00363         if(con == 9){
00364             if(h < LED_COUNT + 1){
00365                 for(int k = 0; k < LED_COUNT; k++){
00366                     uint8_t phase =(k << 1);
00367                     colors[k] = COLOR(phase, 0);
00368                 }
00369                     
00370                 uint8_t phase =(h << 1);
00371                 colors[ledsize - h] = COLOR(phase, P);
00372                 ledStrip1.write(colors, LED_COUNT);
00373             
00374                 if(h == LED_COUNT + 1){
00375                     ticker.detach();
00376                     Xbee2.putc('I');
00377                     con = 0;
00378                 }
00379             }
00380         }
00381         
00382     ledStrip1.write(colors, LED_COUNT);
00383     }
00384 }