robocon_2016
mother
Dependencies: PS3 i2cmaster mbed
Diff: main.cpp
- Revision:
- 1:755d8977d4a3
- Parent:
- 0:a6b6c585a378
--- a/main.cpp Mon Sep 26 10:29:21 2016 +0000
+++ b/main.cpp Fri Sep 30 12:29:16 2016 +0000
@@ -31,6 +31,8 @@
myi2c.WriteSomeData(SE1_addr, check_reg, 0, 1);
myi2c.WriteSomeData(SE2_addr, check_reg, 0, 1);
myi2c.WriteSomeData(CY1_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(SERVO1_addr, check_reg, 0, 1);
+ myi2c.WriteSomeData(0xFE, check_reg, 0, 1);
}
union UNION
@@ -53,6 +55,7 @@
int main()
{
+ BusOut leds(LED1, LED2, LED3, LED4);
NVIC_SetPriority(TIMER0_IRQn, 20);
NVIC_SetPriority(TIMER1_IRQn, 20);
NVIC_SetPriority(TIMER2_IRQn, 20);
@@ -122,14 +125,15 @@
}
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)) ) )
+ leds = modechange;
+ if( modechangeflag && ( sbdbt.getSELECTState() | xbee.getSELECTState() ) )
{
modechange++;
+ if(modechange == 4)modechange = 0;
modechangeflag = 0;
wait_ms(500);
}
- modechangeflag = !( ( sbdbt.getButtonState(sikaku) & sbdbt.getButtonState(sankaku) ) | ( xbee.getButtonState(sikaku) & xbee.getButtonState(sankaku) ) );
+ modechangeflag = !( sbdbt.getSELECTState() | xbee.getSELECTState() );
Maru = sbdbt.getButtonState(maru) | xbee.getButtonState(maru);
Batu = sbdbt.getButtonState(batu) | xbee.getButtonState(batu);
Sankaku = (sbdbt.getButtonState(sankaku) | xbee.getButtonState(sankaku)) & modechangeflag;
@@ -188,7 +192,7 @@
break;
case 1: //box torikomi to bridge
/*Sankaku : torikomi taosu********************************************************/
- myi2c.WriteSomeData(MD6_addr, motor1_pwm, 255, 1);
+ myi2c.WriteSomeData(MD6_addr, motor2_pwm, 255, 1);
if(Sankaku == 0)
{
sankakuflag = 1;
@@ -198,11 +202,11 @@
torikomi_taosu_motor++;
if((torikomi_taosu_motor % 2))
{
- myi2c.WriteSomeData(MD6_addr, motor1_state, 5, 1); //torikomi taosu
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 5, 1); //torikomi taosu
}
else
{
- myi2c.WriteSomeData(MD6_addr, motor1_state, 10, 1); //torikomi modosu
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 10, 1); //torikomi modosu
}
sankakuflag = 0;
@@ -211,33 +215,34 @@
{
sensorsflag[0] = 1;
}
- if(sensorsflag[0] == 1 && sensors[0] == 1)
+ if((sensorsflag[0] == 1 && sensors[0] == 1) || !Sankaku)
{
- myi2c.WriteSomeData(MD6_addr, motor1_state, 3, 1); //torikomi taosi stop
+ myi2c.WriteSomeData(MD6_addr, motor2_state, 3, 1); //torikomi taosi stop
sensorsflag[0] = 0;
}
- /*RightY : torikomi slide**********************************/
- if(RightY > 0)
+ /*LeftY : torikomi slide**********************************/
+ if(LeftY > 0)
{
- myi2c.WriteSomeData(MD6_addr, motor2_state, 5, 1);
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 5, 1);
}
- else if(RightY < 0)
+ else if(LeftY < 0)
{
- myi2c.WriteSomeData(MD6_addr, motor2_state, 10, 1);
- RightY = RightY * (-1);
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 10, 1);
+ LeftY = LeftY * (-1);
}
else
{
- myi2c.WriteSomeData(MD6_addr, motor2_state, 3, 1);
+ myi2c.WriteSomeData(MD6_addr, motor1_state, 3, 1);
}
- myi2c.WriteSomeData(MD6_addr, motor2_pwm, RightY * 4-1, 1);
+ myi2c.WriteSomeData(MD6_addr, motor1_pwm, LeftY * 4-1, 1);
/*Sikaku : cylinder*********************************************/
myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
break;
case 2: //bridge to area3
- /*RightY : bridge1************************************************/
+ myi2c.WriteSomeData(SERVO1_addr, motor1_state, (!Sankaku) | (Sankaku << 1) | ((RightY > 60) << 2) | (Sikaku << 3), 1);
+ /*RightY : bridge1***********************************************
if(RightY > 0)
{
myi2c.WriteSomeData(MD7_addr, motor1_state, 5, 1);
@@ -253,7 +258,7 @@
}
myi2c.WriteSomeData(MD7_addr, motor1_pwm, RightY * 4-1, 1);
- /*LeftY : bridge2*******************************************************/
+ /*LeftY : bridge2******************************************************
if(LeftY > 0)
{
myi2c.WriteSomeData(MD7_addr, motor2_state, 5, 1);
@@ -267,28 +272,31 @@
{
myi2c.WriteSomeData(MD7_addr, motor2_state, 3, 1);
}
- myi2c.WriteSomeData(MD7_addr, motor2_pwm, LeftY * 4-1, 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)
+ if(!Sankaku)
{
- myi2c.WriteSomeData(MD8_addr, motor1_state, 10, 1);
- LeftX = LeftX * (-1);
+ 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);
}
- 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);
+ myi2c.WriteSomeData(MD8_addr, motor2_state, Sikaku * 5, 1);
+ myi2c.WriteSomeData(MD8_addr, motor2_pwm, 255, 1);
/*RightY : rail up***************************************************/
if(RightY > 0)
@@ -313,20 +321,20 @@
/*Sankaku & LeftY : center slide**************************************/
if(Sankaku)
{
- if(LeftX > 0)
+ if(LeftY > 0)
{
myi2c.WriteSomeData(MD10_addr, motor1_state, 5, 1);
}
- else if(LeftX < 0)
+ else if(LeftY < 0)
{
myi2c.WriteSomeData(MD10_addr, motor1_state, 10, 1);
- LeftX = LeftX * (-1);
+ LeftY = LeftY * (-1);
}
else
{
myi2c.WriteSomeData(MD10_addr, motor1_state, 3, 1);
}
- myi2c.WriteSomeData(MD10_addr, motor1_pwm, LeftX * 4-1, 1);
+ myi2c.WriteSomeData(MD10_addr, motor1_pwm, LeftY * 4-1, 1);
}
break;
}