自動機の回収排出の手動プログラム

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "PS3.h"
00004 
00005 PS3 ps3(PA_0,PA_1);
00006 I2C i2c(D14,D15);
00007 Serial pc(USBTX,USBRX);
00008 QEI arm_enc(PB_5, PB_4, NC, 624);
00009 
00010 DigitalIn kaisyu_mae(D3);
00011 DigitalIn kaisyu_usiro(D2);
00012 DigitalIn tyokudo_mae(A3);
00013 DigitalIn tyokudo_usiro(A2);
00014 
00015 char init_send_data[1];
00016 char arm_moter[1],drop_moter[1];
00017 
00018 int phase = 1;
00019 int kaisyu_phase = 0;
00020 int tyokudo_phase = 0;
00021 
00022 void kaisyu(int but, int pulse);
00023 void tyokudo(int but,int pulse);
00024 
00025 int main(void)
00026 {
00027     kaisyu_mae.mode(PullUp);
00028     kaisyu_usiro.mode(PullUp);
00029     tyokudo_mae.mode(PullUp);
00030     tyokudo_usiro.mode(PullUp);
00031     init_send_data[0] = 0x80;
00032     i2c.write(0x18, init_send_data, 1);
00033     i2c.write(0x20, init_send_data, 1);
00034     while(1) {
00035         switch(phase) {
00036             case 1:
00037                 kaisyu(maru,arm_enc.getPulses());
00038                 break;
00039             case 2:
00040                 tyokudo(sikaku,arm_enc.getPulses());
00041                 break;
00042 
00043         }
00044     }
00045 }
00046 
00047 
00048 
00049 void kaisyu(int but, int pulse)
00050 {
00051     //一回ボタン押したら動くやつ(危険)
00052     if(kaisyu_mae == 0 && kaisyu_usiro == 1 && kaisyu_phase == 0) {
00053         kaisyu_phase = ps3.getButtonState(but);
00054     }
00055     switch (kaisyu_phase) {
00056         case 0:
00057             arm_moter[0] = 0x80;
00058             kaisyu_phase = 0;
00059             break;
00060         case 1:
00061             //前進->減速
00062             if(pulse < 2000) {
00063                 arm_moter[0] = 0xFF;
00064                 kaisyu_phase = 1;
00065             } else {
00066                 arm_moter[0] = 0xB3;
00067                 kaisyu_phase = 2;
00068             }
00069             break;
00070         case 2:
00071             //前進->停止->後進
00072             if(kaisyu_usiro == 1) {
00073                 arm_moter[0] = 0xB3;
00074                 kaisyu_phase = 2;
00075             } else {
00076                 arm_moter[0] =  0x80;
00077                 kaisyu_phase = 3;
00078                 i2c.write(0x18, arm_moter,1);
00079                 wait(1);
00080             }
00081             break;
00082         case 3:
00083             //後進->減速
00084             if(pulse > 1600) {
00085                 arm_moter[0] = 0x00;
00086                 kaisyu_phase = 3;
00087             } else {
00088                 arm_moter[0] = 0x4C;
00089                 kaisyu_phase = 4;
00090             }
00091             break;
00092         case 4:
00093             //後進->停止
00094             if(kaisyu_mae == 1) {
00095                 arm_moter[0] = 0x4C;
00096                 kaisyu_phase = 4;
00097             } else {
00098                 arm_moter[0] = 0x80;
00099                 kaisyu_phase = 0;
00100                 phase = 2;
00101             }
00102             break;
00103         default:
00104             break;
00105     }
00106     i2c.write(0x18, arm_moter,1);
00107 }
00108 
00109 void tyokudo(int but,int pulse)
00110 {
00111     //一回ボタン押したら動くやつ(危険)
00112     if(tyokudo_mae == 1 && tyokudo_usiro == 0 && tyokudo_phase == 0) {
00113         tyokudo_phase = ps3.getButtonState(but);
00114     }
00115     switch (tyokudo_phase) {
00116         case 0:
00117             arm_moter[0] = 0x80;
00118             drop_moter[0] = 0x80;
00119             break;
00120         case 1:
00121             //前進->減速
00122             arm_moter[0] = (pulse < 2000)? 0xCD:0xC0;
00123             drop_moter[0] = (pulse < 2000)? 0xE6:0xCD;
00124             tyokudo_phase = (pulse < 2000)? 1:2;
00125             break;
00126         case 2:
00127             //前進(遅い)->停止->後進(早い)
00128             if(tyokudo_mae == 1) {
00129                 arm_moter[0] = 0xC0;
00130                 drop_moter[0] = 0xCD;
00131                 tyokudo_phase = 2;
00132             } else {
00133                 arm_moter[0] = 0x80;
00134                 drop_moter[0] = 0x80;
00135                 tyokudo_phase = 3;
00136                 i2c.write(0x18, arm_moter,1);
00137                 i2c.write(0x20, drop_moter,1);
00138                 wait(1);
00139             }
00140             break;
00141         case 3:
00142             //後進->減速
00143             arm_moter[0] = (pulse > 1600)? 0x33:0x33;
00144             drop_moter[0] = (pulse > 1600)? 0x19:0x19;
00145             tyokudo_phase = (pulse > 1600)? 3:4;
00146             break;
00147         case 4:
00148             //後進->停止
00149             arm_moter[0] = (tyokudo_usiro == 1)? 0x33:0x80;
00150             drop_moter[0] = (tyokudo_usiro == 1)? 0x19:0x80;
00151             tyokudo_phase = (tyokudo_usiro == 1)? 4:0;
00152             break;
00153         default:
00154             arm_moter[0] = 0x80;
00155             drop_moter[0] = 0x80;
00156             tyokudo_phase = 0;
00157             break;
00158     }
00159     i2c.write(0x18, arm_moter,1);
00160     i2c.write(0x20, drop_moter,1);
00161 }