robocon_2016
mother
Dependencies: PS3 i2cmaster mbed
Diff: main.cpp
- Revision:
- 0:a6b6c585a378
- Child:
- 1:755d8977d4a3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Sep 26 10:29:21 2016 +0000
@@ -0,0 +1,493 @@
+#include "mbed.h"
+#include "PS3.h"
+#include "i2cmaster.h"
+#include "define.h"
+
+#define Enter 13
+#define BackSpace 8
+#define Space 32
+
+i2c myi2c(p28, p27);
+
+int i2ccheck[4] = {};
+int timer = 0;
+
+void check()
+{
+ timer++;
+ /*Slave puted data first by mother cause error*/
+ myi2c.WriteSomeData(0xFE, check_reg, 0, 1); //0xFE is imaginary address
+ /*then, I puted imaginary address yhe!*/
+ myi2c.WriteSomeData(MD1_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD2_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD3_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD4_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD5_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD6_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD7_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD8_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD9_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(MD10_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(SE1_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(SE2_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(CY1_addr, check_reg, 0, 1);
+}
+
+union UNION
+{
+ int integer;
+ char character[4];
+};
+UNION PM_target[4];
+UNION PM_ofset[4];
+UNION RE_target[4];
+UNION gain;
+
+Serial pc(USBTX, USBRX);
+int pcdata = 0;
+int prev_pcdata = 0;
+void getpc();
+
+DigitalOut sbdbt_battery(p21);
+DigitalOut xbee_battery(p5);
+
+int main()
+{
+ NVIC_SetPriority(TIMER0_IRQn, 20);
+ NVIC_SetPriority(TIMER1_IRQn, 20);
+ NVIC_SetPriority(TIMER2_IRQn, 20);
+ NVIC_SetPriority(TIMER3_IRQn, 20);
+ NVIC_SetPriority(UART0_IRQn, 10);
+ NVIC_SetPriority(UART1_IRQn, 10);
+ NVIC_SetPriority(UART2_IRQn, 10);
+ NVIC_SetPriority(UART3_IRQn, 10);
+ PS3 sbdbt(p13, p14);
+ PS3 xbee(p9, p10);
+ sbdbt_battery = 1;
+ sbdbt.baud(9600);
+ xbee_battery = 1;
+ xbee.baud(9600);
+ //xbee.addattach(sbdbt_to_xbee);
+ //sbdbt.addattach(xbee_to_sbdbt);
+ pc.attach(getpc);
+ myi2c.frequency(1000000);
+ Ticker t;
+ t.attach(check, 0.1);
+ int prev_PM_target[4] = {};
+ char sensor1 = 0;
+ char sensors[8] = {};
+ char sensorsflag[8] = {};
+ char sensor2 = 0;
+ int gainup = 0;
+ int Dpad_deg = 0;
+ bool Dpadctrl_flag = 1;
+ //0, ue, sita
+ int Dpad_table[3][3] = {{ 0, 0, 180}, //0
+ { 90, 45, 135}, //migi
+ {-90, -45, -135}};//hidari
+ int wheel[4] = {};
+ int torikomi_taosu_motor = 0;
+ int modechange = 0;
+ int modechangeflag = 1;
+ bool counterwheel = 0;
+ bool l1 = 0;
+ bool r1 = 0;
+ bool l2 = 0;
+ bool r2 = 0;
+ bool Maru = 0;
+ bool Batu = 0;
+ bool Sankaku = 0;
+ int sankakuflag = 0;
+ bool Sikaku = 0;
+ bool Ue = 0;
+ bool Sita = 0;
+ bool Migi = 0;
+ bool Hidari = 0;
+ int RightY = 0;
+ int LeftY = 0;
+ int LeftX = 0;
+ wait_ms(500);
+
+ for(int address = 0; address < 4; address++)
+ {
+ while(PM_ofset[address].integer < 1750 || PM_ofset[address].integer > 1850)
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ myi2c.getSlaveRegistarData(MD1_addr + (0x02*address), PM_ofset_reg + i, &PM_ofset[address].character[i], 1);
+ wait_ms(20);
+ }
+ }
+ PM_target[address].integer = PM_ofset[address].integer;
+ }
+ while(1)
+ {
+ printf("%4d %4d %4d %4d", sbdbt.getRightJoystickXaxis(), sbdbt.getRightJoystickYaxis(), sbdbt.getLeftJoystickXaxis(), sbdbt.getLeftJoystickYaxis());
+ if( modechangeflag && ( (sbdbt.getButtonState(sikaku) & sbdbt.getButtonState(sankaku)) | (xbee.getButtonState(sikaku) & xbee.getButtonState(sikaku)) ) )
+ {
+ modechange++;
+ modechangeflag = 0;
+ wait_ms(500);
+ }
+ modechangeflag = !( ( sbdbt.getButtonState(sikaku) & sbdbt.getButtonState(sankaku) ) | ( xbee.getButtonState(sikaku) & xbee.getButtonState(sankaku) ) );
+ Maru = sbdbt.getButtonState(maru) | xbee.getButtonState(maru);
+ Batu = sbdbt.getButtonState(batu) | xbee.getButtonState(batu);
+ Sankaku = (sbdbt.getButtonState(sankaku) | xbee.getButtonState(sankaku)) & modechangeflag;
+ Sikaku = (sbdbt.getButtonState(sikaku) | xbee.getButtonState(sikaku)) & modechangeflag;
+ l1 = sbdbt.getButtonState(L1) | xbee.getButtonState(L1);
+ r1 = sbdbt.getButtonState(R1) | xbee.getButtonState(R1);
+ l2 = sbdbt.getButtonState(L2) | xbee.getButtonState(L2);
+ r2 = sbdbt.getButtonState(R2) | xbee.getButtonState(R2);
+ Ue = sbdbt.getButtonState(ue) | xbee.getButtonState(ue);
+ Sita = sbdbt.getButtonState(sita) | xbee.getButtonState(sita);
+ Migi = sbdbt.getButtonState(migi) | xbee.getButtonState(migi);
+ Hidari = sbdbt.getButtonState(hidari) | xbee.getButtonState(hidari);
+ RightY = sbdbt.getRightJoystickYaxis();
+ LeftY = sbdbt.getLeftJoystickYaxis();
+ LeftX = sbdbt.getLeftJoystickXaxis();
+ for(int i = 0; i < 8; i++)
+ sensors[i] = (sensor1>>i) & 1;
+
+ switch(modechange)
+ {
+ case 0: //start to area1 clear
+ /*LeftY : tukamuyatu spin**********************************/
+ if(LeftY > 0)
+ {
+ myi2c.WriteSomeData(MD5_addr, motor1_state, 5, 1);
+ }
+ else if(LeftY < 0)
+ {
+ myi2c.WriteSomeData(MD5_addr, motor1_state, 10, 1);
+ LeftY = LeftY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD5_addr, motor1_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD5_addr, motor1_pwm, LeftY * 4-1, 1);
+
+ /*RightY : rail slide***********************************/
+ if(RightY > 0)
+ {
+ myi2c.WriteSomeData(MD5_addr, motor2_state, 5, 1);
+ }
+ else if(RightY < 0)
+ {
+ myi2c.WriteSomeData(MD5_addr, motor2_state, 10, 1);
+ RightY = RightY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD5_addr, motor2_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD5_addr, motor2_pwm, RightY * 4-1, 1);
+
+ /*Sikaku : cylinder*****************************************/
+ myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
+ break;
+ case 1: //box torikomi to bridge
+ /*Sankaku : torikomi taosu********************************************************/
+ myi2c.WriteSomeData(MD6_addr, motor1_pwm, 255, 1);
+ if(Sankaku == 0)
+ {
+ sankakuflag = 1;
+ }
+ if(sankakuflag && Sankaku)
+ {
+ torikomi_taosu_motor++;
+ if((torikomi_taosu_motor % 2))
+ {
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 5, 1); //torikomi taosu
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 10, 1); //torikomi modosu
+
+ }
+ sankakuflag = 0;
+ }
+ if(sensors[0] == 0)
+ {
+ sensorsflag[0] = 1;
+ }
+ if(sensorsflag[0] == 1 && sensors[0] == 1)
+ {
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 3, 1); //torikomi taosi stop
+ sensorsflag[0] = 0;
+ }
+
+ /*RightY : torikomi slide**********************************/
+ if(RightY > 0)
+ {
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 5, 1);
+ }
+ else if(RightY < 0)
+ {
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 10, 1);
+ RightY = RightY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD6_addr, motor2_pwm, RightY * 4-1, 1);
+
+ /*Sikaku : cylinder*********************************************/
+ myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
+ break;
+ case 2: //bridge to area3
+ /*RightY : bridge1************************************************/
+ if(RightY > 0)
+ {
+ myi2c.WriteSomeData(MD7_addr, motor1_state, 5, 1);
+ }
+ else if(RightY < 0)
+ {
+ myi2c.WriteSomeData(MD7_addr, motor1_state, 10, 1);
+ RightY = RightY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD7_addr, motor1_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD7_addr, motor1_pwm, RightY * 4-1, 1);
+
+ /*LeftY : bridge2*******************************************************/
+ if(LeftY > 0)
+ {
+ myi2c.WriteSomeData(MD7_addr, motor2_state, 5, 1);
+ }
+ else if(LeftY < 0)
+ {
+ myi2c.WriteSomeData(MD7_addr, motor2_state, 10, 1);
+ LeftY = LeftY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD7_addr, motor2_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD7_addr, motor2_pwm, LeftY * 4-1, 1);
+ break;
+ case 3: //box pile up
+ /*LeftX : jack move*************************************************/
+ if(LeftX > 0)
+ {
+ myi2c.WriteSomeData(MD8_addr, motor1_state, 5, 1);
+ }
+ else if(LeftX < 0)
+ {
+ myi2c.WriteSomeData(MD8_addr, motor1_state, 10, 1);
+ LeftX = LeftX * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD8_addr, motor1_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD8_addr, motor1_pwm, LeftX * 4-1, 1);
+
+ /*Sikaku : jack up***************************************************/
+ myi2c.WriteSomeData(MD8_addr, motor2_state, Sikaku, 1);
+ myi2c.WriteSomeData(MD8_addr, motor2_state, 255, 1);
+
+ /*RightY : rail up***************************************************/
+ if(RightY > 0)
+ {
+ myi2c.WriteSomeData(MD9_addr, motor1_state, 5, 1);
+ myi2c.WriteSomeData(MD9_addr, motor2_state, 5, 1);
+ }
+ else if(RightY < 0)
+ {
+ myi2c.WriteSomeData(MD9_addr, motor1_state, 10, 1);
+ myi2c.WriteSomeData(MD9_addr, motor2_state, 10, 1);
+ RightY = RightY * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD9_addr, motor1_state, 3, 1);
+ myi2c.WriteSomeData(MD9_addr, motor2_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD9_addr, motor1_pwm, RightY * 4-1, 1);
+ myi2c.WriteSomeData(MD9_addr, motor2_pwm, RightY * 4-1, 1);
+
+ /*Sankaku & LeftY : center slide**************************************/
+ if(Sankaku)
+ {
+ if(LeftX > 0)
+ {
+ myi2c.WriteSomeData(MD10_addr, motor1_state, 5, 1);
+ }
+ else if(LeftX < 0)
+ {
+ myi2c.WriteSomeData(MD10_addr, motor1_state, 10, 1);
+ LeftX = LeftX * (-1);
+ }
+ else
+ {
+ myi2c.WriteSomeData(MD10_addr, motor1_state, 3, 1);
+ }
+ myi2c.WriteSomeData(MD10_addr, motor1_pwm, LeftX * 4-1, 1);
+ }
+ break;
+ }
+
+ /*Steering******************************************************/
+ Dpad_deg = Dpad_table[Migi + 2*Hidari][Ue + 2*Sita];
+
+ if(Dpad_deg < -90)
+ {
+ Dpad_deg += 180;
+ counterwheel = 1;
+ }
+ else if(Dpad_deg > 90)
+ {
+ Dpad_deg -= 180;
+ counterwheel = 1;
+ }
+ else
+ {
+ counterwheel = 0;
+ }
+
+
+ if(Ue | Sita | Migi | Hidari)
+ {
+ Dpadctrl_flag = 1;
+ }
+ else
+ {
+ Dpadctrl_flag = 0;
+ }
+ if(Batu)
+ {
+ for(int i = 2; i < 4; i++)
+ {
+ prev_PM_target[i] = PM_target[i].integer;
+ if(Dpadctrl_flag)
+ {
+ //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
+ PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
+ }
+ RE_target[i].integer = 300 * Maru;
+ }
+ }
+ else
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ prev_PM_target[i] = PM_target[i].integer;
+ if(Dpadctrl_flag)
+ {
+ //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
+ PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
+ }
+ RE_target[i].integer = 300 * Maru;
+ }
+ }
+ if(Maru)
+ {
+ wheel[0] = 2 - counterwheel;
+ wheel[1] = 1 + counterwheel;
+ wheel[2] = 1 + counterwheel;
+ wheel[3] = 2 - counterwheel;
+
+ if(l2)
+ {
+ wheel[0] = 1;
+ wheel[1] = 1;
+ wheel[2] = 1;
+ wheel[3] = 1;
+ }
+ if(r2)
+ {
+ wheel[0] = 2;
+ wheel[1] = 2;
+ wheel[2] = 2;
+ wheel[3] = 2;
+ }
+ }
+ /*else if(Batu)
+ {
+ wheel[0] = 1 + counterwheel;
+ wheel[1] = 2 - counterwheel;
+ wheel[2] = 2 - counterwheel;
+ wheel[3] = 1 + counterwheel;
+ }*/
+ else
+ {
+ wheel[0] = 0;
+ wheel[1] = 0;
+ wheel[2] = 0;
+ wheel[3] = 0;
+ }
+ if(r1)
+ {
+ if(Batu)
+ {
+ for(int i = 2; i < 4; i++)
+ PM_target[i].integer += 5;
+ Dpadctrl_flag = 0;
+ }
+ else
+ {
+ for(int i = 0; i < 4; i++)
+ PM_target[i].integer += 5;
+ Dpadctrl_flag = 0;
+ }
+ }
+ else if(l1)
+ {
+ if(Batu)
+ {
+ for(int i = 2; i < 4; i++)
+ PM_target[i].integer -= 5;
+ Dpadctrl_flag = 0;
+ }
+ else
+ {
+ for(int i = 0; i < 4; i++)
+ PM_target[i].integer -= 5;
+ Dpadctrl_flag = 0;
+ }
+ }
+ else if(r2 | l2)
+ {
+ PM_target[0].integer = 45 + PM_ofset[0].integer;
+ PM_target[1].integer = -45 + PM_ofset[1].integer;
+ PM_target[2].integer = 45 + PM_ofset[2].integer;
+ PM_target[3].integer = -45 + PM_ofset[3].integer;
+ Dpadctrl_flag = 0;
+ }
+ /*wheel communication*************************************/
+
+ for(int address = 0; address < 4; address++)
+ {
+ myi2c.WriteSomeData(MD1_addr + (0x02*address), wheel_start, wheel[address], 1);
+ if(PM_target[address].integer != prev_PM_target[address])
+ {
+ myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 0, 1);
+ }
+
+ for(int i = 0; i < 4; i++)
+ {
+ myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_target_reg + i, PM_target[address].character[i], 1);
+ myi2c.WriteSomeData(MD1_addr + (0x02*address), RE_target_reg + i, RE_target[address].character[i], 1);
+ }
+ //myi2c.WriteSomeData(MD1_addr + (0x02*address), pid_start, 1, 1);
+ myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 1, 1);
+ }
+ myi2c.getSlaveRegistarData(SE1_addr, 0, &sensor1, 1);
+ wait_ms(20);
+ myi2c.getSlaveRegistarData(SE2_addr, 0, &sensor2, 1);
+ wait_ms(20);
+ }
+}
+
+void getpc()
+{
+ if(pc.readable())
+ {
+ prev_pcdata = pcdata;
+ pcdata = pc.getc();
+ }
+}