自動機の回収排出の手動プログラム
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 30 2022 06:08:57 by
1.7.2