Taiki Maruyama
kaahddhgfhgfghgf
Diff: System/Process/Process.cpp
- Revision:
- 9:49e851ad441e
- Parent:
- 8:cb53beff4bb2
- Child:
- 10:a81546661fb5
--- 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;
+ }