Diff: System/Process/Process.cpp

Revision:

9:10276c46b467

Parent:

8:cb53beff4bb2

--- a/System/Process/Process.cpp	Sat Feb 24 07:32:11 2018 +0000
+++ b/System/Process/Process.cpp	Wed Jan 29 08:54:06 2020 +0000
@@ -1,7 +1,5 @@
 //2018/02/24のやつ
-
 #include "Process.h"
-
 #include "mbed.h"
 #include "../../Communication/XBee/XBee.h"
 #include "../../Input/Switch/Switch.h"
@@ -9,6 +7,8 @@
 #include "../../Output/Servo/Servo.h"
 
 //_____________________
+
+
 /*---------------- HOW TO WRITE ----------------/
 
     ・motor の割り当てを決める
@@ -21,23 +21,27 @@
 
 /---------------- HOW TO WRITE ----------------*/
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
+bool up_flag = true;
+bool down_flag = true;
+double s = 0;
+double n = 0;
+double ao = 0;
+double aka = 0;
+int zone = 1;//1で青 0で赤
 
 
-
-
+DigitalOut deng1(A7);
+DigitalOut deng2(A6);
 
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
 //_____________________
 
 //#define USE_USB_SERIAL
 #ifdef USE_USB_SERIAL
-Serial pc(SERIAL_TX, SERIAL_RX);
 #endif
 XBEE::ControllerData *controller;
 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
-
 using namespace SWITCH;
-
 void SystemProcess(void) {  
     while(true) {
         controller = XBEE::Controller::GetData();
@@ -71,10 +75,11 @@
 
         if(Switch::CheckPushed(ARM_L))
         {
-            if(controller->Button.L)
+            if(controller->AnalogL.Y != 7)
             {
-                motor[ARM].dir = FOR;
-                motor[ARM].pwm = 80.0;
+                if(controller->AnalogL.Y>7){
+                    motor[ARM].dir = FOR;
+                    motor[ARM].pwm = 80.0;
             }
             if(motor[ARM].dir == BACK)
             {
@@ -90,13 +95,178 @@
 
 /------------------------ HOW TO WRITE ------------------------*/
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
-
+if(zone == 1)
+{
+    n = 53.8;
+    if(controller->Button.ZR || controller->Button.ZL)
+    {
+        if(controller->Button.ZR)
+        {
+        motor[3].dir = FOR;
+        motor[3].pwm = s;
+        }
+        if(controller->Button.ZL)
+        {
+        motor[4].dir = FOR;
+        motor[4].pwm = 52;
+        }
+    }else
+    {
+        motor[3].dir = FREE;
+        motor[4].dir = FREE;
+    }
+}else
+{
+    n = 57.9;
+    if(controller->Button.ZR || controller->Button.ZL)
+    {
+        if(controller->Button.ZR)
+        {
+        motor[3].dir = FOR;
+        motor[3].pwm = 48;
+        }
+        if(controller->Button.ZL)
+        {
+        motor[4].dir = FOR;
+        motor[4].pwm = s;
+        }
+    }else
+    {
+        motor[3].dir = FREE;
+        motor[4].dir = FREE;
+    }
+}   
 
+//足回り
+if(controller->AnalogR.Y == 7)
+{
+     motor[0].dir =FREE;
+}
+else if(controller->AnalogR.Y > 7)
+    {
+        motor[0].dir = FOR;
+        motor[0].pwm = (controller->AnalogR.Y - 7) * 14;
+    }else
+    {
+        motor[0].dir = BACK;
+        motor[0].pwm = (7 - controller->AnalogR.Y) * 14;
+    }
 
+if(controller->AnalogL.Y == 7)
+{
+    motor[1].dir =FREE;
+}else
+{
+    if(controller->AnalogL.Y < 7)
+    {
+        motor[1].dir = FOR;
+        motor[1].pwm = ( 7 - controller->AnalogL.Y) * 14;
+    }else{
+        motor[1].dir = BACK;
+        motor[1].pwm = (controller->AnalogL.Y - 7) * 14;
+    }
+}
 
-
+if(controller->Button.UP || controller->Button.DOWN || controller->Button.LEFT || controller->Button.RIGHT )
+{
+    if(controller->Button.UP)
+    {
+        motor[0].dir = BACK;
+        motor[0].pwm =20;
+        motor[1].dir = FOR;
+        motor[1].pwm =20;
+    }
+    if(controller->Button.DOWN)
+    {
+        motor[0].dir = FOR;
+        motor[0].pwm =77;
+        motor[1].dir = BACK;
+        motor[1].pwm =74;
+    }
+    if(controller->Button.LEFT)
+    {
+        motor[0].dir = BACK;
+        motor[0].pwm =20;
+        motor[1].dir = BACK;
+        motor[1].pwm =20;   
+    }
+    if(controller->Button.RIGHT)
+    {
+        motor[0].dir = BACK;
+        motor[0].pwm =20;
+        motor[1].dir = BACK;
+        motor[1].pwm =20;
+        motor[0].dir = FOR;
+        motor[0].pwm =20;
+        motor[1].dir = FOR;
+        motor[1].pwm =20; 
+    }
+}
 
+if(controller->Button.B)//
+{
+    motor[0].dir = FOR;
+    motor[0].pwm =27;
+    motor[1].dir = BACK;
+    motor[1].pwm =24;
+}else
+{
+    deng1 = 0;
+}
+if(controller->Button.L)
+{
+    deng1 = 1;
+}else
+{
+    deng1 = 0;
+}
+if(controller->Button.R)
+{
+    deng2 = 1;
+}else
+{
+    deng2 = 0;
+}
 
+if(controller->Button.START && up_flag )//速度変更用
+{
+    up_flag = false;
+    s += 0.1;
+}else if(controller->Button.SELECT && down_flag)
+{
+    down_flag = false;
+    s -= 0.1;
+}
+if(!controller->Button.START)
+{
+    up_flag = true;
+}
+if(!controller->Button.SELECT)
+{
+    down_flag = true;
+}
+
+if(controller->Button.HOME)
+{
+    s = n;
+}
+
+if(controller->Button.Y || controller->Button.A)
+{
+    if(controller->Button.Y)
+    {
+        motor[2].dir = FOR;
+        motor[2].pwm =50;
+    }
+    if(controller->Button.A)
+    {
+        motor[2].dir = BACK;
+        motor[2].pwm =50;
+    }
+}else
+{
+    motor[2].dir =FREE;
+}
 
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
 //____________________________