Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_final

aaaaaaaaa

Dependencies:   mbed QEI

System/Process/Process.cpp@15:dfcec98f5aa9, 2018-10-21 (annotated)

Committer:
kishibekairohan
Date:
Sun Oct 21 02:14:15 2018 +0000
Revision:
15:dfcec98f5aa9
Parent:
13:b6e02d6261d7 
aaaaaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/Switch/Switch.h"
7ka884 1:b1219d8ca117 11 #include "../../Input/ColorSensor/ColorSensor.h"
7ka884 1:b1219d8ca117 12 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto 0:669ef71cba68 13 #include "../../Input/Potentiometer/Potentiometer.h"
7ka884 1:b1219d8ca117 14 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto 0:669ef71cba68 15 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 16 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 17 #include "../Using.h"
t_yamamoto 0:669ef71cba68 18
kishibekairohan 2:c015739085d3 19
t_yamamoto 0:669ef71cba68 20 using namespace SWITCH;
kishibekairohan 2:c015739085d3 21 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 22 using namespace ACCELERATIONSENSOR;
7ka884 4:ba9df71868df 23 using namespace PID_SPACE;
7ka884 4:ba9df71868df 24 using namespace ROTARYENCODER;
t_yamamoto 0:669ef71cba68 25
t_yamamoto 0:669ef71cba68 26 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 27 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 28 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 29
t_yamamoto 0:669ef71cba68 30 static bool lock;
t_yamamoto 0:669ef71cba68 31 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 32 static int current;
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 35
t_yamamoto 0:669ef71cba68 36 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 37 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 38 #endif
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 41
t_yamamoto 0:669ef71cba68 42 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 43
kishibekairohan 2:c015739085d3 44 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 45
kishibekairohan 7:e88c5d47a3be 46 //************メカナム********************
kishibekairohan 2:c015739085d3 47
kishibekairohan 2:c015739085d3 48 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 49 {
kishibekairohan 2:c015739085d3 50 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 51 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 52 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 53 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 54 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 55 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 56 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 57 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 58 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 59 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 60 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 61 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 62 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 63 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 64 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 65 };
kishibekairohan 2:c015739085d3 66
kishibekairohan 2:c015739085d3 67 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 68 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 69 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 70 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 71 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 72 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 73 else return BRAKE;
kishibekairohan 2:c015739085d3 74 }
kishibekairohan 2:c015739085d3 75 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 76 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 77 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 78 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 79 }
kishibekairohan 2:c015739085d3 80
kishibekairohan 7:e88c5d47a3be 81 //************メカナム********************
kishibekairohan 7:e88c5d47a3be 82
kishibekairohan 7:e88c5d47a3be 83 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 84
kishibekairohan 2:c015739085d3 85 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 86 int Color_B[3];
kishibekairohan 2:c015739085d3 87 int Color_C[3];
kishibekairohan 2:c015739085d3 88 int Color_D[3];
kishibekairohan 2:c015739085d3 89 int intergration = 50;
kishibekairohan 2:c015739085d3 90
kishibekairohan 12:c09b3e08a316 91 int Avecolor_A[3];
kishibekairohan 12:c09b3e08a316 92 int Avecolor_B[3];
kishibekairohan 12:c09b3e08a316 93 int Avecolor_C[3];
kishibekairohan 12:c09b3e08a316 94 int Avecolor_D[3];
kishibekairohan 12:c09b3e08a316 95
kishibekairohan 12:c09b3e08a316 96 void ColorIn();
kishibekairohan 7:e88c5d47a3be 97 void ColorDetection();
kishibekairohan 12:c09b3e08a316 98 void getcolor();
kishibekairohan 13:b6e02d6261d7 99
kishibekairohan 7:e88c5d47a3be 100 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 101
kishibekairohan 2:c015739085d3 102 //************ライントレース変数*******************
kishibekairohan 13:b6e02d6261d7 103 int PointA[3] = {400, 700, 1000};//赤，緑，青
kishibekairohan 13:b6e02d6261d7 104 int PointB[3] = {400, 700, 1000};//赤，緑，青
kishibekairohan 13:b6e02d6261d7 105 int PointC[3] = {1000, 1700, 2400};//赤，緑，青
kishibekairohan 2:c015739085d3 106
kishibekairohan 7:e88c5d47a3be 107 int startP = 35;
kishibekairohan 7:e88c5d47a3be 108 int downP = 5;
kishibekairohan 8:6fb3723f7747 109
kishibekairohan 8:6fb3723f7747 110 int Asasult = 0;
kishibekairohan 8:6fb3723f7747 111 int Bsasult = 0;
kishibekairohan 8:6fb3723f7747 112 int Csasult = 0;
kishibekairohan 8:6fb3723f7747 113 int Dsasult = 0;
kishibekairohan 8:6fb3723f7747 114
kishibekairohan 13:b6e02d6261d7 115 bool compA = false;
kishibekairohan 13:b6e02d6261d7 116 bool compB = false;
kishibekairohan 13:b6e02d6261d7 117 bool compC = false;
kishibekairohan 13:b6e02d6261d7 118 bool compD = false;
kishibekairohan 13:b6e02d6261d7 119
kishibekairohan 13:b6e02d6261d7 120 bool invationA = false;
kishibekairohan 13:b6e02d6261d7 121 bool invationB = false;
kishibekairohan 13:b6e02d6261d7 122 bool invationC = false;
kishibekairohan 13:b6e02d6261d7 123 bool invationD = false;
kishibekairohan 8:6fb3723f7747 124
kishibekairohan 8:6fb3723f7747 125 Ticker Color_T;
kishibekairohan 13:b6e02d6261d7 126
kishibekairohan 13:b6e02d6261d7 127 void ColorDetection();
kishibekairohan 13:b6e02d6261d7 128 void Color_changeflag();
kishibekairohan 13:b6e02d6261d7 129
kishibekairohan 2:c015739085d3 130 //************ライントレース変数*******************
kishibekairohan 9:f93fc79a49ea 131
kishibekairohan 10:1295d39fec3a 132 //************ROタコン******************
kishibekairohan 9:f93fc79a49ea 133 QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 9:f93fc79a49ea 134 QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 8:6fb3723f7747 135 Ticker get_rpm;
kishibekairohan 13:b6e02d6261d7 136 PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0);
kishibekairohan 8:6fb3723f7747 137 PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
kishibekairohan 9:f93fc79a49ea 138 double rpmX;
kishibekairohan 9:f93fc79a49ea 139 double rpmY;
kishibekairohan 9:f93fc79a49ea 140 double disX;
kishibekairohan 9:f93fc79a49ea 141 double disY;
kishibekairohan 8:6fb3723f7747 142 int palseX;
kishibekairohan 8:6fb3723f7747 143 int palseY;
kishibekairohan 8:6fb3723f7747 144 int RtpwmX;
kishibekairohan 8:6fb3723f7747 145 int RtpwmY;
kishibekairohan 13:b6e02d6261d7 146 double goalX = 900.000;
kishibekairohan 13:b6e02d6261d7 147 double goalY = 700.000;
kishibekairohan 15:dfcec98f5aa9 148
kishibekairohan 15:dfcec98f5aa9 149 //double goalXB = 900.000;
kishibekairohan 15:dfcec98f5aa9 150 //double goalYB = 700.000;
kishibekairohan 8:6fb3723f7747 151 void filip();
kishibekairohan 15:dfcec98f5aa9 152 void filipB();
kishibekairohan 12:c09b3e08a316 153
kishibekairohan 10:1295d39fec3a 154 //************ROタコン******************
kishibekairohan 2:c015739085d3 155
kishibekairohan 7:e88c5d47a3be 156 //************ジャイロ*******************
kishibekairohan 9:f93fc79a49ea 157 bool Angle_flagI = false;
kishibekairohan 11:028a150943b5 158 int Angle;
kishibekairohan 7:e88c5d47a3be 159 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan 7:e88c5d47a3be 160 float rotateY;
kishibekairohan 11:028a150943b5 161 //初期値 -5
kishibekairohan 11:028a150943b5 162 int AngletargetX = 4;
kishibekairohan 11:028a150943b5 163 int AngletargetY = -12;
kishibekairohan 12:c09b3e08a316 164 int AngletargetI = -5;
kishibekairohan 7:e88c5d47a3be 165 //************ジャイロ*******************
kishibekairohan 2:c015739085d3 166
kishibekairohan 9:f93fc79a49ea 167 //************Buzzer******************
kishibekairohan 11:028a150943b5 168 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 11:028a150943b5 169 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 170 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 171 Ticker BuzzerTimer;
kishibekairohan 9:f93fc79a49ea 172 bool Emsflag = false;
kishibekairohan 9:f93fc79a49ea 173 //************Buzzer******************
kishibekairohan 11:028a150943b5 174
kishibekairohan 11:028a150943b5 175 //************TapeLed*****************
kishibekairohan 11:028a150943b5 176 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 177 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 178 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 179 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 180 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 181 //************TapaLed*****************
t_yamamoto 0:669ef71cba68 182 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 183
t_yamamoto 0:669ef71cba68 184 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 185 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 186 static void Process0(void);
t_yamamoto 0:669ef71cba68 187 #endif
t_yamamoto 0:669ef71cba68 188 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 189 static void Process1(void);
t_yamamoto 0:669ef71cba68 190 #endif
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 192 static void Process2(void);
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 195 static void Process3(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 198 static void Process4(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 201 static void Process5(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 204 static void Process6(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 207 static void Process7(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 210 static void Process8(void);
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 213 static void Process9(void);
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #endif
t_yamamoto 0:669ef71cba68 216
t_yamamoto 0:669ef71cba68 217 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 218 {
t_yamamoto 0:669ef71cba68 219 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 11:028a150943b5 220 /*Replace here with the initialization code of your variables.*/
kishibekairohan 9:f93fc79a49ea 221 get_rpm.attach_us(&filip,100);
kishibekairohan 11:028a150943b5 222 buzzer.period(1.0/800);
t_yamamoto 0:669ef71cba68 223
t_yamamoto 0:669ef71cba68 224 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 225
t_yamamoto 0:669ef71cba68 226 lock = true;
t_yamamoto 0:669ef71cba68 227 processChangeComp = true;
t_yamamoto 0:669ef71cba68 228 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 229
t_yamamoto 0:669ef71cba68 230 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 231 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 232 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 233 #endif
t_yamamoto 0:669ef71cba68 234 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 235 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 236 #endif
t_yamamoto 0:669ef71cba68 237 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 238 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 239 #endif
t_yamamoto 0:669ef71cba68 240 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 241 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 242 #endif
t_yamamoto 0:669ef71cba68 243 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 244 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 245 #endif
t_yamamoto 0:669ef71cba68 246 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 247 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 248 #endif
t_yamamoto 0:669ef71cba68 249 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 250 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 251 #endif
t_yamamoto 0:669ef71cba68 252 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 253 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 254 #endif
t_yamamoto 0:669ef71cba68 255 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 256 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 257 #endif
t_yamamoto 0:669ef71cba68 258 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 259 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 260 #endif
t_yamamoto 0:669ef71cba68 261 #endif
t_yamamoto 0:669ef71cba68 262 }
t_yamamoto 0:669ef71cba68 263
t_yamamoto 0:669ef71cba68 264 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 265 {
t_yamamoto 0:669ef71cba68 266 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 267 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 268
t_yamamoto 0:669ef71cba68 269 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 270 {
t_yamamoto 0:669ef71cba68 271 current++;
t_yamamoto 0:669ef71cba68 272 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 273 processChangeComp = false;
t_yamamoto 0:669ef71cba68 274 }
t_yamamoto 0:669ef71cba68 275 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 276 {
t_yamamoto 0:669ef71cba68 277 current--;
t_yamamoto 0:669ef71cba68 278 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 279 processChangeComp = false;
t_yamamoto 0:669ef71cba68 280 }
t_yamamoto 0:669ef71cba68 281 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 282 #endif
t_yamamoto 0:669ef71cba68 283
t_yamamoto 0:669ef71cba68 284 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 285 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 286 #endif
t_yamamoto 0:669ef71cba68 287
t_yamamoto 0:669ef71cba68 288 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 289 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 290 #endif
t_yamamoto 0:669ef71cba68 291
t_yamamoto 0:669ef71cba68 292 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 293 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 294 #endif
t_yamamoto 0:669ef71cba68 295
t_yamamoto 0:669ef71cba68 296 }
t_yamamoto 0:669ef71cba68 297
kishibekairohan 2:c015739085d3 298
kishibekairohan 2:c015739085d3 299
t_yamamoto 0:669ef71cba68 300 void SystemProcess()
t_yamamoto 0:669ef71cba68 301 {
t_yamamoto 0:669ef71cba68 302 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 303
t_yamamoto 0:669ef71cba68 304 while(1)
kishibekairohan 15:dfcec98f5aa9 305 {
kishibekairohan 12:c09b3e08a316 306 getcolor();
kishibekairohan 12:c09b3e08a316 307 pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
kishibekairohan 12:c09b3e08a316 308 pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
kishibekairohan 12:c09b3e08a316 309 pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
kishibekairohan 12:c09b3e08a316 310 pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
kishibekairohan 15:dfcec98f5aa9 311
t_yamamoto 0:669ef71cba68 312 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 313 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 314 #endif
t_yamamoto 0:669ef71cba68 315
t_yamamoto 0:669ef71cba68 316 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 317 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 318 {
t_yamamoto 0:669ef71cba68 319 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 320 AllActuatorReset();
t_yamamoto 0:669ef71cba68 321 lock = true;
kishibekairohan 15:dfcec98f5aa9 322 buzzer = 0.5;
kishibekairohan 15:dfcec98f5aa9 323 BuzzerTimer.attach(BuzzerTimer_func, 0.5);
t_yamamoto 0:669ef71cba68 324 }
t_yamamoto 0:669ef71cba68 325 else
t_yamamoto 0:669ef71cba68 326 #endif
t_yamamoto 0:669ef71cba68 327 {
t_yamamoto 0:669ef71cba68 328
t_yamamoto 0:669ef71cba68 329 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 330 if(!lock)
t_yamamoto 0:669ef71cba68 331 {
t_yamamoto 0:669ef71cba68 332 Process[current]();
t_yamamoto 0:669ef71cba68 333 }
t_yamamoto 0:669ef71cba68 334 else
t_yamamoto 0:669ef71cba68 335 #endif
t_yamamoto 0:669ef71cba68 336 {
t_yamamoto 0:669ef71cba68 337 //ロック時の処理
t_yamamoto 0:669ef71cba68 338 }
t_yamamoto 0:669ef71cba68 339 }
t_yamamoto 0:669ef71cba68 340
kishibekairohan 10:1295d39fec3a 341 if ((EMS_0 || EMS_1) && !Emsflag){
kishibekairohan 11:028a150943b5 342 buzzer = 0.5;
kishibekairohan 9:f93fc79a49ea 343 BuzzerTimer.attach(BuzzerTimer_func, 1.2);
kishibekairohan 9:f93fc79a49ea 344 Emsflag = true;
kishibekairohan 11:028a150943b5 345 ledMode = EMS;
kishibekairohan 11:028a150943b5 346 current = 0;
kishibekairohan 11:028a150943b5 347 tapeLedTimer.attach(TapeLedEms_func, 1.2);
kishibekairohan 11:028a150943b5 348 sendLedData.code = (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 349 }
kishibekairohan 9:f93fc79a49ea 350
kishibekairohan 9:f93fc79a49ea 351 if(!EMS_0 && !EMS_1) {
kishibekairohan 9:f93fc79a49ea 352 buzzer = 0;
kishibekairohan 9:f93fc79a49ea 353 BuzzerTimer.detach();
kishibekairohan 9:f93fc79a49ea 354 Emsflag = false;
kishibekairohan 11:028a150943b5 355 if(ledMode == EMS) ledMode = Normal;
kishibekairohan 11:028a150943b5 356 tapeLedTimer.detach();
kishibekairohan 11:028a150943b5 357 }
kishibekairohan 11:028a150943b5 358
kishibekairohan 11:028a150943b5 359 switch(ledMode)
kishibekairohan 11:028a150943b5 360 {
kishibekairohan 11:028a150943b5 361 case EMS :
kishibekairohan 11:028a150943b5 362 break;
kishibekairohan 11:028a150943b5 363
kishibekairohan 11:028a150943b5 364 case Normal :
kishibekairohan 11:028a150943b5 365 sendLedData.code = tapeLED.code;
kishibekairohan 11:028a150943b5 366
kishibekairohan 11:028a150943b5 367 default:
kishibekairohan 11:028a150943b5 368 break;
kishibekairohan 9:f93fc79a49ea 369 }
kishibekairohan 9:f93fc79a49ea 370
t_yamamoto 0:669ef71cba68 371 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 372 }
t_yamamoto 0:669ef71cba68 373 }
t_yamamoto 0:669ef71cba68 374
kishibekairohan 2:c015739085d3 375
kishibekairohan 2:c015739085d3 376
kishibekairohan 2:c015739085d3 377
t_yamamoto 0:669ef71cba68 378 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 379 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 380 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 381 static void Process0()
kishibekairohan 10:1295d39fec3a 382 {
kishibekairohan 12:c09b3e08a316 383 tapeLED.code = (uint32_t)Green;
kishibekairohan 13:b6e02d6261d7 384 if(RedSW){
kishibekairohan 13:b6e02d6261d7 385 current = 1;
kishibekairohan 13:b6e02d6261d7 386 }
kishibekairohan 13:b6e02d6261d7 387 if(BlueSW){
kishibekairohan 13:b6e02d6261d7 388 current = 2;
kishibekairohan 13:b6e02d6261d7 389 }
t_yamamoto 0:669ef71cba68 390 }
t_yamamoto 0:669ef71cba68 391 #endif
t_yamamoto 0:669ef71cba68 392
t_yamamoto 0:669ef71cba68 393 #if USE_PROCESS_NUM>1
kishibekairohan 12:c09b3e08a316 394 static void Process1() //手動
t_yamamoto 0:669ef71cba68 395 {
kishibekairohan 12:c09b3e08a316 396 tapeLED.code = (uint32_t)Orange;
7ka884 4:ba9df71868df 397 motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 398 motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 399 motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 400 motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 2:c015739085d3 401
kishibekairohan 7:e88c5d47a3be 402 motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 403 motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 404 motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 7:e88c5d47a3be 405 motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 2:c015739085d3 406
kishibekairohan 2:c015739085d3 407 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 7:e88c5d47a3be 408 motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
kishibekairohan 7:e88c5d47a3be 409 motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
7ka884 4:ba9df71868df 410 }
7ka884 4:ba9df71868df 411
kishibekairohan 12:c09b3e08a316 412 if(controller->Button.R){
kishibekairohan 12:c09b3e08a316 413 motor[Angle_R].dir = FOR;
kishibekairohan 12:c09b3e08a316 414 motor[Angle_L].dir = BACK;
kishibekairohan 12:c09b3e08a316 415 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 416 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 417 }else if(controller->Button.L){
kishibekairohan 12:c09b3e08a316 418 motor[Angle_R].dir = BACK;
kishibekairohan 12:c09b3e08a316 419 motor[Angle_L].dir = FOR;
kishibekairohan 12:c09b3e08a316 420 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 421 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 422 }else{
kishibekairohan 12:c09b3e08a316 423 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 424 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 425 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 426 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 427 }
kishibekairohan 12:c09b3e08a316 428
kishibekairohan 12:c09b3e08a316 429 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 12:c09b3e08a316 430 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 431 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 432 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 433 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 434 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 12:c09b3e08a316 435 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 436 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 437 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 438 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 439 }
kishibekairohan 12:c09b3e08a316 440 /*for(int i = 0;i<20;i++){
kishibekairohan 12:c09b3e08a316 441 float y = 0;
kishibekairohan 12:c09b3e08a316 442 y = acc[1]*1000;
kishibekairohan 12:c09b3e08a316 443 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 12:c09b3e08a316 444 Angle += rotateY;
kishibekairohan 12:c09b3e08a316 445 }
kishibekairohan 12:c09b3e08a316 446 Angle = Angle/20;
kishibekairohan 12:c09b3e08a316 447 pc.printf("Y:%d \r\n",Angle);*/
kishibekairohan 2:c015739085d3 448
kishibekairohan 12:c09b3e08a316 449 //wheel.getPulses()...どちらの方向にどれだけ回ったか
kishibekairohan 12:c09b3e08a316 450 //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
kishibekairohan 12:c09b3e08a316 451 //軸が何回転したか
kishibekairohan 12:c09b3e08a316 452 //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
t_yamamoto 0:669ef71cba68 453 }
t_yamamoto 0:669ef71cba68 454 #endif
t_yamamoto 0:669ef71cba68 455
t_yamamoto 0:669ef71cba68 456 #if USE_PROCESS_NUM>2
kishibekairohan 13:b6e02d6261d7 457 static void Process2() //trace
kishibekairohan 13:b6e02d6261d7 458 {
kishibekairohan 13:b6e02d6261d7 459 tapeLED.code = (uint32_t)Yellow;
kishibekairohan 13:b6e02d6261d7 460 static bool color_flag = false;
kishibekairohan 13:b6e02d6261d7 461
kishibekairohan 13:b6e02d6261d7 462 static bool traceon = false;//fase1
kishibekairohan 13:b6e02d6261d7 463 static bool yokofla = false;//fase2
kishibekairohan 13:b6e02d6261d7 464 static bool boxslip = false;//fase3
kishibekairohan 13:b6e02d6261d7 465
kishibekairohan 13:b6e02d6261d7 466 //static bool syu = false;
kishibekairohan 13:b6e02d6261d7 467
kishibekairohan 13:b6e02d6261d7 468 ColorDetection();
kishibekairohan 13:b6e02d6261d7 469 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 470
kishibekairohan 13:b6e02d6261d7 471 if(controller->Button.B && !color_flag)
kishibekairohan 13:b6e02d6261d7 472 {
kishibekairohan 13:b6e02d6261d7 473 traceon = true;
kishibekairohan 13:b6e02d6261d7 474 color_flag = true;
kishibekairohan 13:b6e02d6261d7 475 }
kishibekairohan 13:b6e02d6261d7 476 else if(!controller->Button.B)color_flag = false;
kishibekairohan 13:b6e02d6261d7 477
kishibekairohan 13:b6e02d6261d7 478 if(traceon)
kishibekairohan 13:b6e02d6261d7 479 {
kishibekairohan 13:b6e02d6261d7 480 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 481 if(!invationA && !compA && !invationB && !compB)
kishibekairohan 13:b6e02d6261d7 482 {
kishibekairohan 13:b6e02d6261d7 483 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 484 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 485 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 486 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 487
kishibekairohan 13:b6e02d6261d7 488 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 489 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 490 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 491 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 492
kishibekairohan 13:b6e02d6261d7 493 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 494 }
kishibekairohan 13:b6e02d6261d7 495 else if(invationC && compC && !invationB && !compB)
kishibekairohan 13:b6e02d6261d7 496 {
kishibekairohan 13:b6e02d6261d7 497 for(int i = 0; i<1000; i++){
kishibekairohan 13:b6e02d6261d7 498 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 499 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 500 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 501 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 502
kishibekairohan 13:b6e02d6261d7 503 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 504 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 505 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 506 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 507 }
kishibekairohan 13:b6e02d6261d7 508
kishibekairohan 13:b6e02d6261d7 509 yokofla = true;
kishibekairohan 13:b6e02d6261d7 510 traceon = false;
kishibekairohan 13:b6e02d6261d7 511 }
kishibekairohan 13:b6e02d6261d7 512 }
kishibekairohan 13:b6e02d6261d7 513
kishibekairohan 13:b6e02d6261d7 514
kishibekairohan 13:b6e02d6261d7 515 if(yokofla && !traceon)
kishibekairohan 13:b6e02d6261d7 516 {
kishibekairohan 13:b6e02d6261d7 517 //pointcalculation();
kishibekairohan 13:b6e02d6261d7 518 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 519 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 520 {
kishibekairohan 13:b6e02d6261d7 521 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 522 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 523 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 524 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 525
kishibekairohan 13:b6e02d6261d7 526 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 527 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 528 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 529 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 530
kishibekairohan 13:b6e02d6261d7 531 wait(2);
kishibekairohan 13:b6e02d6261d7 532
kishibekairohan 13:b6e02d6261d7 533 boxslip = true;
kishibekairohan 13:b6e02d6261d7 534 yokofla = false;
kishibekairohan 13:b6e02d6261d7 535 }
kishibekairohan 13:b6e02d6261d7 536 else if(compA && compB && compC)
kishibekairohan 13:b6e02d6261d7 537 {
kishibekairohan 13:b6e02d6261d7 538 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 539 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 540 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 541 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 542
kishibekairohan 13:b6e02d6261d7 543 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 544 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 545 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 546 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 547
kishibekairohan 13:b6e02d6261d7 548 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 549 }
kishibekairohan 13:b6e02d6261d7 550 else if(invationA && invationB && invationC)
kishibekairohan 13:b6e02d6261d7 551 {
kishibekairohan 13:b6e02d6261d7 552 motor[TIRE_FR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 553 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 554 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 555 motor[TIRE_BL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 556
kishibekairohan 13:b6e02d6261d7 557 //motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 558 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 559 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 560 //motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 561 }
kishibekairohan 13:b6e02d6261d7 562 else if(!invationA && !invationB && !invationC)
kishibekairohan 13:b6e02d6261d7 563 {
kishibekairohan 13:b6e02d6261d7 564 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 565 motor[TIRE_FL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 566 motor[TIRE_BR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 567 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 568
kishibekairohan 13:b6e02d6261d7 569 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 570 //motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 571 //motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 572 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 573 }
kishibekairohan 13:b6e02d6261d7 574 else if(!invationA && compC && invationC)//C固定A下
kishibekairohan 13:b6e02d6261d7 575 {
kishibekairohan 13:b6e02d6261d7 576 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 577 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 578 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 579 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 580
kishibekairohan 13:b6e02d6261d7 581 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 582 motor[TIRE_FL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 583 motor[TIRE_BR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 584 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 585
kishibekairohan 13:b6e02d6261d7 586 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 587 }
kishibekairohan 13:b6e02d6261d7 588 else if(compA && compB && !invationC)//AB固定C下
kishibekairohan 13:b6e02d6261d7 589 {
kishibekairohan 13:b6e02d6261d7 590 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 591 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 592 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 593 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 594
kishibekairohan 13:b6e02d6261d7 595 motor[TIRE_FR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 596 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 597 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 598 motor[TIRE_BL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 599
kishibekairohan 13:b6e02d6261d7 600 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 601 }
kishibekairohan 13:b6e02d6261d7 602 else if(compA && compB && !compC && invationC)//AB固定C上
kishibekairohan 13:b6e02d6261d7 603 {
kishibekairohan 13:b6e02d6261d7 604 motor[TIRE_FR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 605 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 606 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 607 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 608
kishibekairohan 13:b6e02d6261d7 609 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 610 motor[TIRE_FL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 611 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 612 motor[TIRE_BL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 613
kishibekairohan 13:b6e02d6261d7 614 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 615 }
kishibekairohan 13:b6e02d6261d7 616 else if(!compA && invationA && compC)//C固定A上
kishibekairohan 13:b6e02d6261d7 617 {
kishibekairohan 13:b6e02d6261d7 618 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 619 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 620 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 621 motor[TIRE_BL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 622
kishibekairohan 13:b6e02d6261d7 623 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 624 motor[TIRE_FL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 625 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 626 motor[TIRE_BL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 627
kishibekairohan 13:b6e02d6261d7 628 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 629 }
kishibekairohan 13:b6e02d6261d7 630 }
kishibekairohan 13:b6e02d6261d7 631
kishibekairohan 13:b6e02d6261d7 632 if(boxslip)
kishibekairohan 13:b6e02d6261d7 633 {
kishibekairohan 13:b6e02d6261d7 634 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 635 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 636 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 637 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 638
kishibekairohan 13:b6e02d6261d7 639 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 640 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 641 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 642 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 643 }
kishibekairohan 13:b6e02d6261d7 644 }
kishibekairohan 13:b6e02d6261d7 645 #endif
kishibekairohan 13:b6e02d6261d7 646
kishibekairohan 13:b6e02d6261d7 647 #if USE_PROCESS_NUM>3
kishibekairohan 15:dfcec98f5aa9 648 static void Process3() //Blue Zone
kishibekairohan 13:b6e02d6261d7 649 {
kishibekairohan 15:dfcec98f5aa9 650 filipB();
kishibekairohan 15:dfcec98f5aa9 651
kishibekairohan 15:dfcec98f5aa9 652 static bool Rt_flagX = false;
kishibekairohan 15:dfcec98f5aa9 653 static bool Rt_flagY = false;
kishibekairohan 15:dfcec98f5aa9 654
kishibekairohan 15:dfcec98f5aa9 655 if(Rt_flagX)
kishibekairohan 15:dfcec98f5aa9 656 {
kishibekairohan 15:dfcec98f5aa9 657 filipB();
kishibekairohan 15:dfcec98f5aa9 658 if(disX < goalX - 5){
kishibekairohan 15:dfcec98f5aa9 659 filipB();
kishibekairohan 15:dfcec98f5aa9 660
kishibekairohan 15:dfcec98f5aa9 661 motor[TIRE_FR].dir = SetStatus(-RtpwmX);
kishibekairohan 15:dfcec98f5aa9 662 motor[TIRE_FL].dir = SetStatus(-RtpwmX);
kishibekairohan 15:dfcec98f5aa9 663 motor[TIRE_BR].dir = SetStatus(RtpwmX);
kishibekairohan 15:dfcec98f5aa9 664 motor[TIRE_BL].dir = SetStatus(RtpwmX);
kishibekairohan 15:dfcec98f5aa9 665 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
kishibekairohan 15:dfcec98f5aa9 666 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan 15:dfcec98f5aa9 667 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan 15:dfcec98f5aa9 668 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan 15:dfcec98f5aa9 669 }
kishibekairohan 15:dfcec98f5aa9 670 else if(disX > goalX - 5){
kishibekairohan 15:dfcec98f5aa9 671
kishibekairohan 15:dfcec98f5aa9 672 for(int i = 0; i<200; i++){
kishibekairohan 15:dfcec98f5aa9 673 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 674 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 675 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 676 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 677
kishibekairohan 15:dfcec98f5aa9 678 motor[TIRE_FR].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 679 motor[TIRE_FL].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 680 motor[TIRE_BR].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 681 motor[TIRE_BL].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 682 }
kishibekairohan 15:dfcec98f5aa9 683
kishibekairohan 15:dfcec98f5aa9 684 Rt_flagY = true;
kishibekairohan 15:dfcec98f5aa9 685 Rt_flagX = false;
kishibekairohan 15:dfcec98f5aa9 686 }
kishibekairohan 15:dfcec98f5aa9 687 }
kishibekairohan 15:dfcec98f5aa9 688
kishibekairohan 15:dfcec98f5aa9 689 if(Rt_flagY && !Rt_flagX){
kishibekairohan 15:dfcec98f5aa9 690 filipB();
kishibekairohan 15:dfcec98f5aa9 691 if(disY < goalY - 5){
kishibekairohan 15:dfcec98f5aa9 692 filipB();
kishibekairohan 15:dfcec98f5aa9 693 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 15:dfcec98f5aa9 694 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 695 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 15:dfcec98f5aa9 696 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 697 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 698 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 699 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 700 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan 15:dfcec98f5aa9 701 }
kishibekairohan 15:dfcec98f5aa9 702 else if(disY > goalY - 5)
kishibekairohan 15:dfcec98f5aa9 703 {
kishibekairohan 15:dfcec98f5aa9 704 filipB();
kishibekairohan 15:dfcec98f5aa9 705 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 706 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 707 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 708 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 15:dfcec98f5aa9 709 motor[TIRE_FR].pwm = 255*0.85;
kishibekairohan 15:dfcec98f5aa9 710 motor[TIRE_FL].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 711 motor[TIRE_BR].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 712 motor[TIRE_BL].pwm = 255;
kishibekairohan 15:dfcec98f5aa9 713 }
kishibekairohan 15:dfcec98f5aa9 714 }
kishibekairohan 13:b6e02d6261d7 715 }
kishibekairohan 13:b6e02d6261d7 716 #endif
kishibekairohan 13:b6e02d6261d7 717
kishibekairohan 13:b6e02d6261d7 718 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 719 static void Process4()
kishibekairohan 12:c09b3e08a316 720 {
kishibekairohan 15:dfcec98f5aa9 721
kishibekairohan 13:b6e02d6261d7 722 }
kishibekairohan 13:b6e02d6261d7 723 #endif
kishibekairohan 13:b6e02d6261d7 724
kishibekairohan 13:b6e02d6261d7 725 #if USE_PROCESS_NUM>5
kishibekairohan 13:b6e02d6261d7 726 static void Process5() //ロタコンXY
kishibekairohan 13:b6e02d6261d7 727 {
kishibekairohan 13:b6e02d6261d7 728 tapeLED.code = (uint32_t)White;
kishibekairohan 13:b6e02d6261d7 729
kishibekairohan 13:b6e02d6261d7 730 static bool nopushed = false;
kishibekairohan 13:b6e02d6261d7 731 static bool Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 732 static bool Rt_flagY = false;
kishibekairohan 13:b6e02d6261d7 733
kishibekairohan 13:b6e02d6261d7 734 if(controller->Button.A && !nopushed){
kishibekairohan 13:b6e02d6261d7 735 Rt_flagX = true;
kishibekairohan 13:b6e02d6261d7 736 nopushed = true;
kishibekairohan 13:b6e02d6261d7 737
kishibekairohan 13:b6e02d6261d7 738 RtX.reset();
kishibekairohan 13:b6e02d6261d7 739 RtY.reset();
kishibekairohan 13:b6e02d6261d7 740 }else if(!controller->Button.A)nopushed = false;
kishibekairohan 13:b6e02d6261d7 741
kishibekairohan 13:b6e02d6261d7 742 filip();
kishibekairohan 13:b6e02d6261d7 743
kishibekairohan 13:b6e02d6261d7 744 if(Rt_flagX)
kishibekairohan 13:b6e02d6261d7 745 {
kishibekairohan 13:b6e02d6261d7 746 filip();
kishibekairohan 13:b6e02d6261d7 747 if(disX < goalX - 5){
kishibekairohan 13:b6e02d6261d7 748 filip();
kishibekairohan 13:b6e02d6261d7 749
kishibekairohan 13:b6e02d6261d7 750 motor[TIRE_FR].dir = SetStatus(-RtpwmX);
kishibekairohan 13:b6e02d6261d7 751 motor[TIRE_FL].dir = SetStatus(-RtpwmX);
kishibekairohan 13:b6e02d6261d7 752 motor[TIRE_BR].dir = SetStatus(RtpwmX);
kishibekairohan 13:b6e02d6261d7 753 motor[TIRE_BL].dir = SetStatus(RtpwmX);
kishibekairohan 13:b6e02d6261d7 754 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
kishibekairohan 13:b6e02d6261d7 755 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 756 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 757 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 758 }
kishibekairohan 13:b6e02d6261d7 759 else if(disX > goalX - 5){
kishibekairohan 13:b6e02d6261d7 760
kishibekairohan 13:b6e02d6261d7 761 for(int i = 0; i<200; i++){
kishibekairohan 13:b6e02d6261d7 762 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 763 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 764 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 765 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 766
kishibekairohan 13:b6e02d6261d7 767 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 768 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 769 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 770 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 771 }
kishibekairohan 13:b6e02d6261d7 772
kishibekairohan 13:b6e02d6261d7 773 Rt_flagY = true;
kishibekairohan 13:b6e02d6261d7 774 Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 775 }
kishibekairohan 13:b6e02d6261d7 776 }
kishibekairohan 13:b6e02d6261d7 777
kishibekairohan 13:b6e02d6261d7 778
kishibekairohan 13:b6e02d6261d7 779 if(Rt_flagY && !Rt_flagX){
kishibekairohan 13:b6e02d6261d7 780 filip();
kishibekairohan 13:b6e02d6261d7 781 if(disY < goalY - 5){
kishibekairohan 13:b6e02d6261d7 782 filip();
kishibekairohan 13:b6e02d6261d7 783 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 784 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 785 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 786 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 787 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 788 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 789 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 790 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 791 }
kishibekairohan 13:b6e02d6261d7 792 else if(disY > goalY - 5)
kishibekairohan 13:b6e02d6261d7 793 {
kishibekairohan 13:b6e02d6261d7 794 filip();
kishibekairohan 13:b6e02d6261d7 795 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 796 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 797 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 798 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 799 motor[TIRE_FR].pwm = 255*0.85;
kishibekairohan 13:b6e02d6261d7 800 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 801 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 802 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 803 }
kishibekairohan 13:b6e02d6261d7 804 }
kishibekairohan 13:b6e02d6261d7 805 }
kishibekairohan 13:b6e02d6261d7 806 #endif
kishibekairohan 13:b6e02d6261d7 807
kishibekairohan 13:b6e02d6261d7 808 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 809 static void Process6()
kishibekairohan 13:b6e02d6261d7 810 {
kishibekairohan 13:b6e02d6261d7 811 tapeLED.code = (uint32_t)Yellow;
kishibekairohan 13:b6e02d6261d7 812 static bool color_flag = false;
kishibekairohan 13:b6e02d6261d7 813
kishibekairohan 13:b6e02d6261d7 814 static bool traceon = false;//fase1
kishibekairohan 13:b6e02d6261d7 815 static bool yokofla = false;//fase2
kishibekairohan 13:b6e02d6261d7 816 static bool boxslip = false;//fase3
kishibekairohan 13:b6e02d6261d7 817
kishibekairohan 13:b6e02d6261d7 818 static bool nopushed = false;
kishibekairohan 13:b6e02d6261d7 819 static bool Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 820 static bool Rt_flagY = false;
kishibekairohan 13:b6e02d6261d7 821
kishibekairohan 13:b6e02d6261d7 822 //static bool syu = false;
kishibekairohan 13:b6e02d6261d7 823
kishibekairohan 13:b6e02d6261d7 824 ColorDetection();
kishibekairohan 13:b6e02d6261d7 825 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 826
kishibekairohan 13:b6e02d6261d7 827 if(controller->Button.B && !color_flag)
kishibekairohan 13:b6e02d6261d7 828 {
kishibekairohan 13:b6e02d6261d7 829 traceon = true;
kishibekairohan 13:b6e02d6261d7 830 color_flag = true;
kishibekairohan 13:b6e02d6261d7 831 }
kishibekairohan 13:b6e02d6261d7 832 else if(!controller->Button.B)color_flag = false;
kishibekairohan 13:b6e02d6261d7 833
kishibekairohan 13:b6e02d6261d7 834 if(traceon)
kishibekairohan 13:b6e02d6261d7 835 {
kishibekairohan 13:b6e02d6261d7 836 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 837 if(!invationA && !compA && !invationB && !compB)
kishibekairohan 13:b6e02d6261d7 838 {
kishibekairohan 13:b6e02d6261d7 839 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 840 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 841 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 842 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 843
kishibekairohan 13:b6e02d6261d7 844 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 845 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 846 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 847 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 848
kishibekairohan 13:b6e02d6261d7 849 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 850 }
kishibekairohan 13:b6e02d6261d7 851 else if(invationC && compC && !invationB && !compB)
kishibekairohan 13:b6e02d6261d7 852 {
kishibekairohan 13:b6e02d6261d7 853 for(int i = 0; i<1000; i++){
kishibekairohan 13:b6e02d6261d7 854 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 855 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 856 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 857 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 858
kishibekairohan 13:b6e02d6261d7 859 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 860 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 861 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 862 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 863 }
kishibekairohan 13:b6e02d6261d7 864
kishibekairohan 13:b6e02d6261d7 865 yokofla = true;
kishibekairohan 13:b6e02d6261d7 866 traceon = false;
kishibekairohan 13:b6e02d6261d7 867 }
kishibekairohan 13:b6e02d6261d7 868 }
kishibekairohan 13:b6e02d6261d7 869
kishibekairohan 13:b6e02d6261d7 870
kishibekairohan 13:b6e02d6261d7 871 if(yokofla && !traceon)
kishibekairohan 13:b6e02d6261d7 872 {
kishibekairohan 13:b6e02d6261d7 873 //pointcalculation();
kishibekairohan 13:b6e02d6261d7 874 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 875 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 876 {
kishibekairohan 13:b6e02d6261d7 877 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 878 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 879 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 880 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 881
kishibekairohan 13:b6e02d6261d7 882 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 883 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 884 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 885 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 886
kishibekairohan 13:b6e02d6261d7 887 wait(2);
kishibekairohan 13:b6e02d6261d7 888
kishibekairohan 13:b6e02d6261d7 889 boxslip = true;
kishibekairohan 13:b6e02d6261d7 890 yokofla = false;
kishibekairohan 13:b6e02d6261d7 891 }
kishibekairohan 13:b6e02d6261d7 892 else if(compA && compB && compC)
kishibekairohan 13:b6e02d6261d7 893 {
kishibekairohan 13:b6e02d6261d7 894 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 895 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 896 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 897 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 898
kishibekairohan 13:b6e02d6261d7 899 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 900 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 901 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 902 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 903
kishibekairohan 13:b6e02d6261d7 904 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 905 }
kishibekairohan 13:b6e02d6261d7 906 else if(invationA && invationB && invationC)
kishibekairohan 13:b6e02d6261d7 907 {
kishibekairohan 13:b6e02d6261d7 908 motor[TIRE_FR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 909 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 910 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 911 motor[TIRE_BL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 912
kishibekairohan 13:b6e02d6261d7 913 //motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 914 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 915 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 916 //motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 917 }
kishibekairohan 13:b6e02d6261d7 918 else if(!invationA && !invationB && !invationC)
kishibekairohan 13:b6e02d6261d7 919 {
kishibekairohan 13:b6e02d6261d7 920 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 921 motor[TIRE_FL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 922 motor[TIRE_BR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 923 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 924
kishibekairohan 13:b6e02d6261d7 925 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 926 //motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 927 //motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 928 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 929 }
kishibekairohan 13:b6e02d6261d7 930 else if(!invationA && compC && invationC)//C固定A下
kishibekairohan 13:b6e02d6261d7 931 {
kishibekairohan 13:b6e02d6261d7 932 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 933 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 934 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 935 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 936
kishibekairohan 13:b6e02d6261d7 937 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 938 motor[TIRE_FL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 939 motor[TIRE_BR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 940 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 941
kishibekairohan 13:b6e02d6261d7 942 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 943 }
kishibekairohan 13:b6e02d6261d7 944 else if(compA && compB && !invationC)//AB固定C下
kishibekairohan 13:b6e02d6261d7 945 {
kishibekairohan 13:b6e02d6261d7 946 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 947 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 948 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 949 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 950
kishibekairohan 13:b6e02d6261d7 951 motor[TIRE_FR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 952 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 953 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 954 motor[TIRE_BL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 955
kishibekairohan 13:b6e02d6261d7 956 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 957 }
kishibekairohan 13:b6e02d6261d7 958 else if(compA && compB && !compC && invationC)//AB固定C上
kishibekairohan 13:b6e02d6261d7 959 {
kishibekairohan 13:b6e02d6261d7 960 motor[TIRE_FR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 961 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 962 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 963 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 964
kishibekairohan 13:b6e02d6261d7 965 motor[TIRE_FR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 966 motor[TIRE_FL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 967 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 968 motor[TIRE_BL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 969
kishibekairohan 13:b6e02d6261d7 970 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 971 }
kishibekairohan 13:b6e02d6261d7 972 else if(!compA && invationA && compC)//C固定A上
kishibekairohan 13:b6e02d6261d7 973 {
kishibekairohan 13:b6e02d6261d7 974 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 975 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 976 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 977 motor[TIRE_BL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 978
kishibekairohan 13:b6e02d6261d7 979 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 980 motor[TIRE_FL].pwm = 100;
kishibekairohan 13:b6e02d6261d7 981 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 982 motor[TIRE_BL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 983
kishibekairohan 13:b6e02d6261d7 984 Color_changeflag();
kishibekairohan 13:b6e02d6261d7 985 }
kishibekairohan 13:b6e02d6261d7 986 }
kishibekairohan 13:b6e02d6261d7 987
kishibekairohan 13:b6e02d6261d7 988 if(boxslip)
kishibekairohan 13:b6e02d6261d7 989 {
kishibekairohan 13:b6e02d6261d7 990 for(int i = 0; i<500; i++)
kishibekairohan 13:b6e02d6261d7 991 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 992 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 993 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 994 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 995
kishibekairohan 13:b6e02d6261d7 996 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 997 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 998 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 999 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1000
kishibekairohan 13:b6e02d6261d7 1001 Rt_flagX = true;
kishibekairohan 13:b6e02d6261d7 1002 nopushed = true;
kishibekairohan 13:b6e02d6261d7 1003
kishibekairohan 13:b6e02d6261d7 1004 RtX.reset();
kishibekairohan 13:b6e02d6261d7 1005 RtY.reset();
kishibekairohan 13:b6e02d6261d7 1006 }
kishibekairohan 13:b6e02d6261d7 1007
kishibekairohan 13:b6e02d6261d7 1008
kishibekairohan 13:b6e02d6261d7 1009 /*if(controller->Button.A && !nopushed){
kishibekairohan 13:b6e02d6261d7 1010 Rt_flagX = true;
kishibekairohan 13:b6e02d6261d7 1011 nopushed = true;
kishibekairohan 13:b6e02d6261d7 1012
kishibekairohan 13:b6e02d6261d7 1013 RtX.reset();
kishibekairohan 13:b6e02d6261d7 1014 RtY.reset();
kishibekairohan 13:b6e02d6261d7 1015 }else if(!controller->Button.A)nopushed = false;
kishibekairohan 13:b6e02d6261d7 1016 */
kishibekairohan 13:b6e02d6261d7 1017 filip();
kishibekairohan 13:b6e02d6261d7 1018
kishibekairohan 13:b6e02d6261d7 1019 if(Rt_flagX)
kishibekairohan 13:b6e02d6261d7 1020 {
kishibekairohan 13:b6e02d6261d7 1021 filip();
kishibekairohan 13:b6e02d6261d7 1022 if(disX < goalX - 5){
kishibekairohan 13:b6e02d6261d7 1023 filip();
kishibekairohan 13:b6e02d6261d7 1024
kishibekairohan 13:b6e02d6261d7 1025 motor[TIRE_FR].dir = SetStatus(-RtpwmX);
kishibekairohan 13:b6e02d6261d7 1026 motor[TIRE_FL].dir = SetStatus(-RtpwmX);
kishibekairohan 13:b6e02d6261d7 1027 motor[TIRE_BR].dir = SetStatus(RtpwmX);
kishibekairohan 13:b6e02d6261d7 1028 motor[TIRE_BL].dir = SetStatus(RtpwmX);
kishibekairohan 13:b6e02d6261d7 1029 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
kishibekairohan 13:b6e02d6261d7 1030 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 1031 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 1032 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 1033 }
kishibekairohan 13:b6e02d6261d7 1034 else if(disX > goalX - 5){
kishibekairohan 13:b6e02d6261d7 1035
kishibekairohan 13:b6e02d6261d7 1036 for(int i = 0; i<500; i++){
kishibekairohan 13:b6e02d6261d7 1037 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1038 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1039 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1040 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1041
kishibekairohan 13:b6e02d6261d7 1042 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1043 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1044 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1045 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1046 }
kishibekairohan 13:b6e02d6261d7 1047
kishibekairohan 13:b6e02d6261d7 1048 Rt_flagY = true;
kishibekairohan 13:b6e02d6261d7 1049 Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 1050 }
kishibekairohan 13:b6e02d6261d7 1051 }
kishibekairohan 13:b6e02d6261d7 1052
kishibekairohan 13:b6e02d6261d7 1053
kishibekairohan 13:b6e02d6261d7 1054 if(Rt_flagY && !Rt_flagX){
kishibekairohan 13:b6e02d6261d7 1055 filip();
kishibekairohan 13:b6e02d6261d7 1056 if(disY < goalY - 5){
kishibekairohan 13:b6e02d6261d7 1057 filip();
kishibekairohan 13:b6e02d6261d7 1058 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1059 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1060 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1061 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1062 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1063 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1064 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1065 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1066 }
kishibekairohan 13:b6e02d6261d7 1067 else if(disY > goalY - 5)
kishibekairohan 13:b6e02d6261d7 1068 {
kishibekairohan 13:b6e02d6261d7 1069 filip();
kishibekairohan 13:b6e02d6261d7 1070 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1071 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1072 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1073 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1074 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1075 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1076 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1077 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1078 }
kishibekairohan 13:b6e02d6261d7 1079 }
kishibekairohan 13:b6e02d6261d7 1080
kishibekairohan 13:b6e02d6261d7 1081 }
kishibekairohan 13:b6e02d6261d7 1082 #endif
kishibekairohan 13:b6e02d6261d7 1083
kishibekairohan 13:b6e02d6261d7 1084 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 1085 static void Process7()
kishibekairohan 13:b6e02d6261d7 1086 {
kishibekairohan 12:c09b3e08a316 1087 tapeLED.code = (uint32_t)Hotpink;
kishibekairohan 12:c09b3e08a316 1088 static bool Xnopush = false;
kishibekairohan 12:c09b3e08a316 1089 static bool Ynopush = false;
kishibekairohan 12:c09b3e08a316 1090 static bool Inopush = false;
kishibekairohan 12:c09b3e08a316 1091
kishibekairohan 12:c09b3e08a316 1092 static bool Angle_flagX = false;
kishibekairohan 12:c09b3e08a316 1093 static bool Angle_flagY = false;
kishibekairohan 12:c09b3e08a316 1094 static bool ANgle_flagI = false;
kishibekairohan 12:c09b3e08a316 1095
kishibekairohan 12:c09b3e08a316 1096 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 12:c09b3e08a316 1097 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1098 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1099 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1100 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1101 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 12:c09b3e08a316 1102 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1103 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1104 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1105 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1106 }
kishibekairohan 12:c09b3e08a316 1107 for(int i = 0;i<20;i++){
kishibekairohan 12:c09b3e08a316 1108 float y = 0;
kishibekairohan 12:c09b3e08a316 1109 y = acc[1]*1000;
kishibekairohan 12:c09b3e08a316 1110 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 12:c09b3e08a316 1111 Angle += rotateY;
kishibekairohan 12:c09b3e08a316 1112 }
kishibekairohan 12:c09b3e08a316 1113 Angle = Angle /20;
kishibekairohan 12:c09b3e08a316 1114
kishibekairohan 12:c09b3e08a316 1115 int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan 12:c09b3e08a316 1116 int gyropwmY = gyro.SetPV(Angle,AngletargetY);
kishibekairohan 12:c09b3e08a316 1117 int gyropwmI = gyro.SetPV(Angle,AngletargetI);
kishibekairohan 12:c09b3e08a316 1118
kishibekairohan 12:c09b3e08a316 1119 if(controller->Button.X && !Xnopush){
kishibekairohan 12:c09b3e08a316 1120 Angle_flagX = true;
kishibekairohan 12:c09b3e08a316 1121 Xnopush = true;
kishibekairohan 12:c09b3e08a316 1122 }else if(!controller->Button.X)Xnopush = false;
kishibekairohan 12:c09b3e08a316 1123
kishibekairohan 12:c09b3e08a316 1124 if(controller->Button.Y && !Ynopush){
kishibekairohan 12:c09b3e08a316 1125 Angle_flagY = true;
kishibekairohan 12:c09b3e08a316 1126 Ynopush = true;
kishibekairohan 12:c09b3e08a316 1127 }else if(!controller->Button.Y)Ynopush = false;
kishibekairohan 12:c09b3e08a316 1128
kishibekairohan 12:c09b3e08a316 1129 if(controller->Button.A && !Inopush){
kishibekairohan 12:c09b3e08a316 1130 Angle_flagI = true;
kishibekairohan 12:c09b3e08a316 1131 Inopush = true;
kishibekairohan 12:c09b3e08a316 1132 }else if(!controller->Button.A)Inopush = false;
kishibekairohan 12:c09b3e08a316 1133
kishibekairohan 12:c09b3e08a316 1134 if (Angle_flagX){
kishibekairohan 12:c09b3e08a316 1135 motor[Angle_R].dir = SetStatus(gyropwmX);
kishibekairohan 12:c09b3e08a316 1136 motor[Angle_L].dir = SetStatus(-gyropwmX);
kishibekairohan 12:c09b3e08a316 1137 motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan 12:c09b3e08a316 1138 motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan 12:c09b3e08a316 1139 if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan 12:c09b3e08a316 1140 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1141 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1142 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1143 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1144 Angle_flagX = false;
kishibekairohan 12:c09b3e08a316 1145 }
kishibekairohan 12:c09b3e08a316 1146 }
kishibekairohan 12:c09b3e08a316 1147
kishibekairohan 12:c09b3e08a316 1148 if (Angle_flagY){
kishibekairohan 12:c09b3e08a316 1149 motor[Angle_R].dir = SetStatus(-gyropwmY);
kishibekairohan 12:c09b3e08a316 1150 motor[Angle_L].dir = SetStatus(gyropwmY);
kishibekairohan 12:c09b3e08a316 1151 motor[Angle_R].pwm = SetPWM(gyropwmY);
kishibekairohan 12:c09b3e08a316 1152 motor[Angle_L].pwm = SetPWM(gyropwmY);
kishibekairohan 12:c09b3e08a316 1153 if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
kishibekairohan 12:c09b3e08a316 1154 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1155 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1156 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1157 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1158 Angle_flagY = false;
kishibekairohan 12:c09b3e08a316 1159 }
kishibekairohan 12:c09b3e08a316 1160 }
kishibekairohan 12:c09b3e08a316 1161
kishibekairohan 12:c09b3e08a316 1162 if (Angle_flagI){
kishibekairohan 12:c09b3e08a316 1163 if(Angle < 0)
kishibekairohan 12:c09b3e08a316 1164 {
kishibekairohan 12:c09b3e08a316 1165 motor[Angle_R].dir = SetStatus(-gyropwmI);
kishibekairohan 12:c09b3e08a316 1166 motor[Angle_L].dir = SetStatus(gyropwmI);
kishibekairohan 12:c09b3e08a316 1167 motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1168 motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1169
kishibekairohan 12:c09b3e08a316 1170 if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan 12:c09b3e08a316 1171 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1172 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1173 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1174 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1175 Angle_flagI = false;
kishibekairohan 12:c09b3e08a316 1176 }
kishibekairohan 12:c09b3e08a316 1177 }
kishibekairohan 12:c09b3e08a316 1178 else if(Angle > 0)
kishibekairohan 12:c09b3e08a316 1179 {
kishibekairohan 12:c09b3e08a316 1180 motor[Angle_R].dir = FOR;
kishibekairohan 12:c09b3e08a316 1181 motor[Angle_L].dir = BACK;
kishibekairohan 12:c09b3e08a316 1182 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 1183 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 1184
kishibekairohan 12:c09b3e08a316 1185 if(Angle < 0){
kishibekairohan 12:c09b3e08a316 1186 motor[Angle_R].dir = SetStatus(gyropwmI);
kishibekairohan 12:c09b3e08a316 1187 motor[Angle_L].dir = SetStatus(-gyropwmI);
kishibekairohan 12:c09b3e08a316 1188 motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1189 motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1190
kishibekairohan 12:c09b3e08a316 1191 if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan 12:c09b3e08a316 1192 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1193 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1194 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1195 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1196 Angle_flagI = false;
kishibekairohan 12:c09b3e08a316 1197 }
kishibekairohan 12:c09b3e08a316 1198 }
kishibekairohan 12:c09b3e08a316 1199 }
kishibekairohan 12:c09b3e08a316 1200 }
kishibekairohan 12:c09b3e08a316 1201 else{
kishibekairohan 12:c09b3e08a316 1202 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1203 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1204 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1205 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1206 }
kishibekairohan 12:c09b3e08a316 1207 }
kishibekairohan 12:c09b3e08a316 1208 #endif
kishibekairohan 12:c09b3e08a316 1209
kishibekairohan 13:b6e02d6261d7 1210 #if USE_PROCESS_NUM>8 //kakudo
kishibekairohan 13:b6e02d6261d7 1211 static void Process8()
kishibekairohan 12:c09b3e08a316 1212 {
kishibekairohan 12:c09b3e08a316 1213
kishibekairohan 12:c09b3e08a316 1214 }
kishibekairohan 12:c09b3e08a316 1215 #endif
kishibekairohan 12:c09b3e08a316 1216
kishibekairohan 13:b6e02d6261d7 1217 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 1218 static void Process9()
kishibekairohan 12:c09b3e08a316 1219 {
kishibekairohan 2:c015739085d3 1220
t_yamamoto 0:669ef71cba68 1221 }
t_yamamoto 0:669ef71cba68 1222 #endif
t_yamamoto 0:669ef71cba68 1223 #endif
t_yamamoto 0:669ef71cba68 1224 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1225
t_yamamoto 0:669ef71cba68 1226 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1227 {
t_yamamoto 0:669ef71cba68 1228
t_yamamoto 0:669ef71cba68 1229 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 1230 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 1231 #endif
t_yamamoto 0:669ef71cba68 1232
t_yamamoto 0:669ef71cba68 1233 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 1234 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 1235 {
t_yamamoto 0:669ef71cba68 1236 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 1237 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 1238 }
t_yamamoto 0:669ef71cba68 1239 #endif
t_yamamoto 0:669ef71cba68 1240 }
t_yamamoto 0:669ef71cba68 1241
t_yamamoto 0:669ef71cba68 1242 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 8:6fb3723f7747 1243
kishibekairohan 8:6fb3723f7747 1244 void filip(){
kishibekairohan 8:6fb3723f7747 1245 palseX = RtX.getPulses();
kishibekairohan 8:6fb3723f7747 1246 palseY = RtY.getPulses();
7ka884 6:10e22bc327ce 1247
kishibekairohan 9:f93fc79a49ea 1248 rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1249 rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1250
kishibekairohan 10:1295d39fec3a 1251 disX = 48*3.141*rpmX;
kishibekairohan 9:f93fc79a49ea 1252 disY = 48*3.141*rpmY;
kishibekairohan 9:f93fc79a49ea 1253
kishibekairohan 10:1295d39fec3a 1254 RtpwmX = (int)Rt_X.SetPV(disX , goalX);
kishibekairohan 9:f93fc79a49ea 1255 RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
kishibekairohan 8:6fb3723f7747 1256 }
7ka884 6:10e22bc327ce 1257
kishibekairohan 15:dfcec98f5aa9 1258 void filipB(){
kishibekairohan 15:dfcec98f5aa9 1259 palseX = RtX.getPulses();
kishibekairohan 15:dfcec98f5aa9 1260 palseY = RtY.getPulses();
kishibekairohan 15:dfcec98f5aa9 1261
kishibekairohan 15:dfcec98f5aa9 1262 rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 15:dfcec98f5aa9 1263 rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 15:dfcec98f5aa9 1264
kishibekairohan 15:dfcec98f5aa9 1265 disX = abs(48*3.141*rpmX);
kishibekairohan 15:dfcec98f5aa9 1266 disY = 48*3.141*rpmY;
kishibekairohan 15:dfcec98f5aa9 1267
kishibekairohan 15:dfcec98f5aa9 1268 RtpwmX = (int)Rt_X.SetPV(disX , goalX);
kishibekairohan 15:dfcec98f5aa9 1269 RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
kishibekairohan 15:dfcec98f5aa9 1270 }
kishibekairohan 15:dfcec98f5aa9 1271
kishibekairohan 12:c09b3e08a316 1272 unsigned long ColorIn(int index)
kishibekairohan 12:c09b3e08a316 1273 {
kishibekairohan 12:c09b3e08a316 1274 int result = 0;
kishibekairohan 12:c09b3e08a316 1275 bool rtn = false;
kishibekairohan 12:c09b3e08a316 1276 for(int i=0; i<12; i++)
kishibekairohan 12:c09b3e08a316 1277 {
kishibekairohan 12:c09b3e08a316 1278 CK[index] = 1;
kishibekairohan 12:c09b3e08a316 1279 rtn = DOUT[index];
kishibekairohan 12:c09b3e08a316 1280 CK[index] = 0;
kishibekairohan 12:c09b3e08a316 1281 if(rtn)
kishibekairohan 12:c09b3e08a316 1282 {
kishibekairohan 12:c09b3e08a316 1283 result|=(1 << i);
kishibekairohan 12:c09b3e08a316 1284 }
kishibekairohan 12:c09b3e08a316 1285 }
kishibekairohan 12:c09b3e08a316 1286 return result;
kishibekairohan 12:c09b3e08a316 1287 }
7ka884 6:10e22bc327ce 1288
kishibekairohan 2:c015739085d3 1289 void ColorDetection(){
kishibekairohan 2:c015739085d3 1290 GATE = 0;
kishibekairohan 2:c015739085d3 1291
kishibekairohan 2:c015739085d3 1292 CK[0] = 0;
kishibekairohan 2:c015739085d3 1293 CK[1] = 0;
kishibekairohan 2:c015739085d3 1294 CK[2] = 0;
kishibekairohan 2:c015739085d3 1295 CK[3] = 0;
kishibekairohan 2:c015739085d3 1296
kishibekairohan 2:c015739085d3 1297 RANGE = 1;
kishibekairohan 2:c015739085d3 1298
kishibekairohan 2:c015739085d3 1299 GATE = 1;
kishibekairohan 2:c015739085d3 1300 wait_ms(intergration);
kishibekairohan 2:c015739085d3 1301 GATE = 0;
kishibekairohan 2:c015739085d3 1302 wait_us(4);
kishibekairohan 2:c015739085d3 1303
kishibekairohan 2:c015739085d3 1304 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 1305 wait_us(3);
kishibekairohan 2:c015739085d3 1306 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 1307 wait_us(3);
kishibekairohan 2:c015739085d3 1308 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 1309
kishibekairohan 2:c015739085d3 1310 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 1311 wait_us(3);
kishibekairohan 2:c015739085d3 1312 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 1313 wait_us(3);
kishibekairohan 2:c015739085d3 1314 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 1315
kishibekairohan 2:c015739085d3 1316 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 1317 wait_us(3);
kishibekairohan 2:c015739085d3 1318 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 1319 wait_us(3);
kishibekairohan 2:c015739085d3 1320 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 1321
kishibekairohan 2:c015739085d3 1322 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 1323 wait_us(3);
kishibekairohan 2:c015739085d3 1324 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 1325 wait_us(3);
kishibekairohan 2:c015739085d3 1326 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 1327 }
kishibekairohan 12:c09b3e08a316 1328
kishibekairohan 13:b6e02d6261d7 1329 void Color_changeflag(){
kishibekairohan 13:b6e02d6261d7 1330 ColorDetection();
kishibekairohan 13:b6e02d6261d7 1331
kishibekairohan 13:b6e02d6261d7 1332 if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
kishibekairohan 13:b6e02d6261d7 1333 {
kishibekairohan 13:b6e02d6261d7 1334 invationA ^= 1;//start false,over true
kishibekairohan 13:b6e02d6261d7 1335 compA = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1336 }
kishibekairohan 13:b6e02d6261d7 1337 else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶
kishibekairohan 13:b6e02d6261d7 1338
kishibekairohan 13:b6e02d6261d7 1339 if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
kishibekairohan 13:b6e02d6261d7 1340 {
kishibekairohan 13:b6e02d6261d7 1341 invationB ^= 1;//start false,over true
kishibekairohan 13:b6e02d6261d7 1342 compB = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1343 }
kishibekairohan 13:b6e02d6261d7 1344 else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶
kishibekairohan 13:b6e02d6261d7 1345
kishibekairohan 13:b6e02d6261d7 1346 if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
kishibekairohan 13:b6e02d6261d7 1347 {
kishibekairohan 13:b6e02d6261d7 1348 invationC ^= 1;//start false,over true
kishibekairohan 13:b6e02d6261d7 1349 compC = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1350 }
kishibekairohan 13:b6e02d6261d7 1351 else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶
kishibekairohan 13:b6e02d6261d7 1352 /*
kishibekairohan 13:b6e02d6261d7 1353 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan 13:b6e02d6261d7 1354 {
kishibekairohan 13:b6e02d6261d7 1355 invationD ^= 1;//start false,over true
kishibekairohan 13:b6e02d6261d7 1356 compD = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1357 }
kishibekairohan 13:b6e02d6261d7 1358 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
kishibekairohan 13:b6e02d6261d7 1359 */
kishibekairohan 13:b6e02d6261d7 1360 }
kishibekairohan 13:b6e02d6261d7 1361
kishibekairohan 12:c09b3e08a316 1362 void getcolor(){
kishibekairohan 13:b6e02d6261d7 1363 for(int i=0;i<10;i++){
kishibekairohan 12:c09b3e08a316 1364 ColorDetection();
kishibekairohan 12:c09b3e08a316 1365
kishibekairohan 12:c09b3e08a316 1366 Avecolor_A[0] += Color_A[0];
kishibekairohan 12:c09b3e08a316 1367 Avecolor_A[1] += Color_A[1];
kishibekairohan 12:c09b3e08a316 1368 Avecolor_A[2] += Color_A[2];
kishibekairohan 12:c09b3e08a316 1369 Avecolor_B[0] += Color_B[0];
kishibekairohan 12:c09b3e08a316 1370 Avecolor_B[1] += Color_B[1];
kishibekairohan 12:c09b3e08a316 1371 Avecolor_B[2] += Color_B[2];
kishibekairohan 12:c09b3e08a316 1372 Avecolor_C[0] += Color_C[0];
kishibekairohan 12:c09b3e08a316 1373 Avecolor_C[1] += Color_C[1];
kishibekairohan 12:c09b3e08a316 1374 Avecolor_C[2] += Color_C[2];
kishibekairohan 12:c09b3e08a316 1375 Avecolor_D[0] += Color_D[0];
kishibekairohan 12:c09b3e08a316 1376 Avecolor_D[1] += Color_D[1];
kishibekairohan 12:c09b3e08a316 1377 Avecolor_D[2] += Color_D[2];
kishibekairohan 12:c09b3e08a316 1378 }
kishibekairohan 12:c09b3e08a316 1379
kishibekairohan 13:b6e02d6261d7 1380 Avecolor_A[0] = Avecolor_A[0]/10;
kishibekairohan 13:b6e02d6261d7 1381 Avecolor_A[1] = Avecolor_A[1]/10;
kishibekairohan 13:b6e02d6261d7 1382 Avecolor_A[2] = Avecolor_A[2]/10;
kishibekairohan 13:b6e02d6261d7 1383 Avecolor_B[0] = Avecolor_B[0]/10;
kishibekairohan 13:b6e02d6261d7 1384 Avecolor_B[1] = Avecolor_B[1]/10;
kishibekairohan 13:b6e02d6261d7 1385 Avecolor_B[2] = Avecolor_B[2]/10;
kishibekairohan 13:b6e02d6261d7 1386 Avecolor_C[0] = Avecolor_C[0]/10;
kishibekairohan 13:b6e02d6261d7 1387 Avecolor_C[1] = Avecolor_C[1]/10;
kishibekairohan 13:b6e02d6261d7 1388 Avecolor_C[2] = Avecolor_C[2]/10;
kishibekairohan 13:b6e02d6261d7 1389 Avecolor_D[0] = Avecolor_D[0]/10;
kishibekairohan 13:b6e02d6261d7 1390 Avecolor_D[1] = Avecolor_D[1]/10;
kishibekairohan 13:b6e02d6261d7 1391 Avecolor_D[2] = Avecolor_D[2]/10;
kishibekairohan 13:b6e02d6261d7 1392
kishibekairohan 12:c09b3e08a316 1393 }
kishibekairohan 12:c09b3e08a316 1394
kishibekairohan 9:f93fc79a49ea 1395 void BuzzerTimer_func() {
kishibekairohan 11:028a150943b5 1396 if(buzzer == 0.5){
kishibekairohan 11:028a150943b5 1397 buzzer = 0;
kishibekairohan 11:028a150943b5 1398 }
kishibekairohan 11:028a150943b5 1399 else if(buzzer == 0){
kishibekairohan 11:028a150943b5 1400 buzzer = 0.5;
kishibekairohan 11:028a150943b5 1401 }
kishibekairohan 11:028a150943b5 1402 }
kishibekairohan 11:028a150943b5 1403
kishibekairohan 11:028a150943b5 1404 void TapeLedEms_func() {
kishibekairohan 11:028a150943b5 1405 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1406 }
t_yamamoto 0:669ef71cba68 1407 #pragma endregion