Revision:
Parent:
8:cb53beff4bb2
```--- a/System/Process/Process.cpp	Sat Feb 24 07:32:11 2018 +0000
+++ b/System/Process/Process.cpp	Sat Feb 23 02:47:39 2019 +0000
@@ -15,16 +15,58 @@
#define TIRE_L 1

・リミットスイッチの割り当てを決める
-        #define ARM_L 1
+        #define 0_L 1

・他にも自由に定義してもいいです (pwmとか)

/---------------- HOW TO WRITE ----------------*/
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓

+#define TIRE_LF 0
+#define TIRE_LB 1
+#define TIRE_RB 2
+#define TIRE_RF 3
+#define ARM_M   4
+
+#define Air1_pin A7
+#define Air2_pin A6
+#define Air3_pin A5
+#define ARM_T 0
+
+const int omni[15][15]=
+{
+    {   0,     2,      6,     15,     26,     41,     59,     80,     80,     80,     80,     80,    80,    80,    80},
+    {  -2,     0,      2,      6,     15,     26,     41,     59,     61,     65,     73,     80,    80,    80,    80},
+    {  -6,    -2,      0,      2,      6,     15,     26,     41,     43,     47,     56,     67,    80,    80,    80},
+    { -15,    -6,     -2,      0,      2,      6,     15,     26,     28,     33,     41,     52,    67,    80,    80},
+    { -26,   -15,     -6,     -2,      0,      2,      6,     15,     17,     21,     29,     41,    56,    73,    80},
+    { -41,   -26,    -15,     -6,     -2,      0,      2,      6,      8,     13,     21,     33,    48,    65,    80},
+    { -59,   -41,    -26,    -15,     -6,     -2,      0,      2,      4,      8,     16,     28,    42,    61,    80},
+    { -80,   -59,    -41,    -26,    -15,     -6,     -2,      0,      2,      6,     15,     26,    41,    59,    80},
+    { -80,   -61,    -42,    -28,    -16,     -8,     -4,     -2,      0,      2,      6,     15,    26,    41,    59},
+    { -80,   -65,    -48,    -33,    -21,    -13,     -8,     -6,     -2,      0,      2,      6,    15,    26,    41},
+    { -80,   -73,    -56,    -41,    -29,    -21,    -17,    -15,     -6,     -2,      0,      2,     6,    15,    26},
+    { -80,   -80,    -67,    -52,    -41,    -33,    -28,    -26,    -15,     -6,     -2,      0,     2,     6,    15},
+    { -80,   -80,    -80,    -67,    -56,    -47,    -43,    -41,    -26,    -15,     -6,     -2,     0,     2,     6},
+    { -80,   -80,    -80,    -80,    -73,    -65,    -61,    -59,    -41,    -26,    -15,     -6,    -2,     0,     2},
+    { -80,   -80,    -80,    -80,    -80,    -80,    -80,    -80,    -59,    -41,    -26,    -15,    -6,    -2,     0}
+};
+
+const int curve[15] = {-64, -47, -33, -21, -12, -5, -1, 0, 1, 5, 12, 21, 33, 47, 64};

+uint8_t SetStatus(int);
+uint8_t SetStatus(int pwmVal){
+    if(pwmVal < 0) return BACK;
+    else if(pwmVal > 0) return FOR;
+    else if(pwmVal == 0) return BRAKE;
+}
+uint8_t SetPWM(int);
+uint8_t SetPWM(int pwmVal){
+    if(pwmVal == 0 || pwmVal >  80 || pwmVal < -80) return 80;
+    else return abs(pwmVal);
+}

//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
//_____________________
@@ -36,8 +78,26 @@
XBEE::ControllerData *controller;
MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];

+
+
using namespace SWITCH;
+/////////////////////////////////
+DigitalOut AirPin[] = {
+    DigitalOut(Air1_pin),
+    DigitalOut(Air2_pin),
+    DigitalOut(Air3_pin),
+};

+void AirOut(int pin,int mode){
+    if(mode==1){
+        AirPin[pin]=1;
+    }else if(mode==0){
+        AirPin[pin]=0;
+    }else{
+        AirPin[pin]=0;
+    }
+}
+//////////////////////////////////
void SystemProcess(void) {
while(true) {
controller = XBEE::Controller::GetData();
@@ -69,16 +129,16 @@

・リミットスイッチの値をもとに動作のプログラムを書く

-        if(Switch::CheckPushed(ARM_L))
+        if(Switch::CheckPushed(0_L))
{
if(controller->Button.L)
{
-                motor[ARM].dir = FOR;
-                motor[ARM].pwm = 80.0;
+                motor[0].dir = FOR;
+                motor[0].pwm = 80.0;
}
-            if(motor[ARM].dir == BACK)
+            if(motor[0].dir == BACK)
{
-                motor[ARM].dir = BRAKE;
+                motor[0].dir = BRAKE;
}
}

@@ -90,10 +150,67 @@

/------------------------ HOW TO WRITE ------------------------*/
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
-
-
-
-
+
+
+    if(controller->AnalogL.X >= 6 && controller->AnalogL.X <= 8 && controller->AnalogL.Y >= 6 && controller->AnalogL.Y <= 8){
+          motor[TIRE_LF].dir = FREE;
+          motor[TIRE_LB].dir = FREE;
+          motor[TIRE_RB].dir = FREE;
+          motor[TIRE_RF].dir = FREE;
+    }
+
+    if(controller->AnalogR.X >= 6 && controller->AnalogR.X <= 8 && controller->AnalogR.Y >= 6 && controller->AnalogR.Y <= 8){
+          motor[TIRE_LF].dir = FREE;
+          motor[TIRE_LB].dir = FREE;
+          motor[TIRE_RB].dir = FREE;
+          motor[TIRE_RF].dir = FREE;
+    }
+
+    motor[TIRE_RF].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
+    motor[TIRE_LF].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
+    motor[TIRE_RB].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
+    motor[TIRE_LB].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
+
+    motor[TIRE_RF].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X])    ;
+    motor[TIRE_LF].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X])       ;
+    motor[TIRE_RB].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
+    motor[TIRE_LB].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y])    ;
+
+    if(controller->Button.R){
+        AirOut(0,1);
+    }
+    if(controller->Button.L){
+        AirOut(0,0);
+    }
+
+    if(controller->Button.A){
+        AirOut(1,1);
+    }
+    if(controller->Button.B){
+        AirOut(1,0);
+    }
+
+    if(controller->Button.X){
+        AirOut(2,1);
+    }
+    if(controller->Button.Y){
+        AirOut(2,0);
+    }
+
+    if(controller->Button.UP) {
+        motor[ARM_M].dir = BACK;
+        motor[ARM_M].pwm = 50;
+    }
+    if(controller->Button.DOWN){
+        motor[ARM_M].dir = FOR;
+        motor[ARM_M].pwm = 50;
+    }
+    if(Switch::checkPushed(ARM_T)){
+        motor[ARM_M].dir = BRAKE;
+    }
+    else{
+        motor[ARM_M].dir = BRAKE;
+    }

```