robocon_2016 / Mbed 2 deprecated 2016_mother_3

mother

Dependencies:   PS3 i2cmaster mbed

Files at this revision

API Documentation at this revision

Revision 1:755d8977d4a3, committed 2016-09-30

Comitter:
sgrsn
Date:
Fri Sep 30 12:29:16 2016 +0000
Parent:
0:a6b6c585a378
Commit message:
mother_9/30

Changed in this revision

PS3.lib Show annotated file Show diff for this revision Revisions of this file
define.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/PS3.lib	Mon Sep 26 10:29:21 2016 +0000
+++ b/PS3.lib	Fri Sep 30 12:29:16 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/sgrsn/code/PS3/#fe29007fb56c
+https://developer.mbed.org/users/sgrsn/code/PS3/#5640aae53391

--- a/define.h	Mon Sep 26 10:29:21 2016 +0000
+++ b/define.h	Fri Sep 30 12:29:16 2016 +0000
@@ -35,6 +35,9 @@
 //CY : Cylinder
 #define CY1_addr 0x50
 
+//SERVO : Servo motor
+#define SERVO1_addr 0x60
+
 //SE : Sensor
 #define SE1_addr 0x90
 #define SE2_addr 0xA0

--- 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;
         }