Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_final

aaaaaaaaa

Dependencies:   mbed QEI

System/Process/Process.cpp@5:3ae504b88679, 2018-10-01 (annotated)

Committer:
kishibekairohan
Date:
Mon Oct 01 09:07:27 2018 +0000
Revision:
5:3ae504b88679
Parent:
3:e10d8736fd22 
maruyama

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
t_yamamoto 0:669ef71cba68 3
t_yamamoto 0:669ef71cba68 4 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
t_yamamoto 0:669ef71cba68 5 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 6 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Input/Switch/Switch.h"
7ka884 1:b1219d8ca117 8 #include "../../Input/ColorSensor/ColorSensor.h"
7ka884 1:b1219d8ca117 9 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/Potentiometer/Potentiometer.h"
7ka884 1:b1219d8ca117 11 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto 0:669ef71cba68 12 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 13 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 14 #include "../Using.h"
kishibekairohan 5:3ae504b88679 15 #include "../../Communication/PID/PID.h"
t_yamamoto 0:669ef71cba68 16
kishibekairohan 2:c015739085d3 17
t_yamamoto 0:669ef71cba68 18 using namespace SWITCH;
kishibekairohan 2:c015739085d3 19 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 20 using namespace ACCELERATIONSENSOR;
kishibekairohan 5:3ae504b88679 21 using namespace PID_SPACE;
kishibekairohan 5:3ae504b88679 22 using namespace ROTARYENCODER;
t_yamamoto 0:669ef71cba68 23
t_yamamoto 0:669ef71cba68 24 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 27
t_yamamoto 0:669ef71cba68 28 static bool lock;
t_yamamoto 0:669ef71cba68 29 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 30 static int current;
t_yamamoto 0:669ef71cba68 31
t_yamamoto 0:669ef71cba68 32 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 35 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 36 #endif
t_yamamoto 0:669ef71cba68 37
t_yamamoto 0:669ef71cba68 38 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 41
kishibekairohan 2:c015739085d3 42 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 43
kishibekairohan 2:c015739085d3 44 unsigned long ColorIn(int index)
kishibekairohan 2:c015739085d3 45 {
kishibekairohan 2:c015739085d3 46 int result = 0;
kishibekairohan 2:c015739085d3 47 bool rtn = false;
kishibekairohan 2:c015739085d3 48 for(int i=0; i<12; i++)
kishibekairohan 2:c015739085d3 49 {
kishibekairohan 2:c015739085d3 50 CK[index] = 1;
kishibekairohan 2:c015739085d3 51 rtn = DOUT[index];
kishibekairohan 2:c015739085d3 52 CK[index] = 0;
kishibekairohan 2:c015739085d3 53 if(rtn)
kishibekairohan 2:c015739085d3 54 {
kishibekairohan 2:c015739085d3 55 result|=(1 << i);
kishibekairohan 2:c015739085d3 56 }
kishibekairohan 2:c015739085d3 57 }
kishibekairohan 2:c015739085d3 58 return result;
kishibekairohan 2:c015739085d3 59 }
kishibekairohan 2:c015739085d3 60
kishibekairohan 5:3ae504b88679 61 #define TIRE_FR 0 //足回り前右
kishibekairohan 5:3ae504b88679 62 #define TIRE_FL 1 //足回り前左
kishibekairohan 5:3ae504b88679 63 #define TIRE_BR 2 //足回り後右
kishibekairohan 5:3ae504b88679 64 #define TIRE_BL 3 //足回り後左
kishibekairohan 2:c015739085d3 65
kishibekairohan 5:3ae504b88679 66 #define Angle_R 4 //角度調節右
kishibekairohan 5:3ae504b88679 67 #define Angle_L 5 //角度調節左
kishibekairohan 5:3ae504b88679 68
kishibekairohan 5:3ae504b88679 69 #define Lim_AR 3 //角度調節右
kishibekairohan 5:3ae504b88679 70 #define Lim_AL 4 //角度調節左
kishibekairohan 5:3ae504b88679 71 #define Lim_R 0 //センター右
kishibekairohan 5:3ae504b88679 72 #define Lim_L 1 //センター左
kishibekairohan 5:3ae504b88679 73
kishibekairohan 5:3ae504b88679 74 //************メカナム********************
kishibekairohan 2:c015739085d3 75
kishibekairohan 2:c015739085d3 76 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 77 {
kishibekairohan 2:c015739085d3 78 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 79 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 80 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 81 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 82 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 83 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 84 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 85 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 86 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 87 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 88 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 89 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 90 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 91 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 92 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 93 };
kishibekairohan 2:c015739085d3 94
kishibekairohan 2:c015739085d3 95 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 96 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 97 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 98 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 99 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 100 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 101 else return BRAKE;
kishibekairohan 2:c015739085d3 102 }
kishibekairohan 2:c015739085d3 103 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 104 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 105 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 106 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 107 }
kishibekairohan 2:c015739085d3 108
kishibekairohan 5:3ae504b88679 109 //************メカナム********************
kishibekairohan 5:3ae504b88679 110
kishibekairohan 5:3ae504b88679 111 //************カラーセンサ変数********************
kishibekairohan 2:c015739085d3 112 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 113 int Color_B[3];
kishibekairohan 2:c015739085d3 114 int Color_C[3];
kishibekairohan 2:c015739085d3 115 int Color_D[3];
kishibekairohan 2:c015739085d3 116 int intergration = 50;
kishibekairohan 5:3ae504b88679 117 /*int averageR_A;
kishibekairohan 5:3ae504b88679 118 int averageG_A;
kishibekairohan 5:3ae504b88679 119 int averageB_A;
kishibekairohan 5:3ae504b88679 120 int averageR_B;
kishibekairohan 5:3ae504b88679 121 int averageG_B;
kishibekairohan 5:3ae504b88679 122 int averageB_B;
kishibekairohan 5:3ae504b88679 123 int averageR_C;
kishibekairohan 5:3ae504b88679 124 int averageG_C;
kishibekairohan 5:3ae504b88679 125 int averageB_C;
kishibekairohan 5:3ae504b88679 126 int averageR_D;
kishibekairohan 5:3ae504b88679 127 int averageG_D;
kishibekairohan 5:3ae504b88679 128 int averageB_D;*/
kishibekairohan 2:c015739085d3 129
kishibekairohan 2:c015739085d3 130 void ColorDetection();
kishibekairohan 2:c015739085d3 131
kishibekairohan 5:3ae504b88679 132 /*DigitalIn www(RT11_PIN);
kishibekairohan 5:3ae504b88679 133 DigitalIn mmm(RT12_PIN);
kishibekairohan 5:3ae504b88679 134
kishibekairohan 5:3ae504b88679 135 int pp = 0;
kishibekairohan 5:3ae504b88679 136 int bb = 0;
kishibekairohan 5:3ae504b88679 137
kishibekairohan 5:3ae504b88679 138 */
kishibekairohan 5:3ae504b88679 139
kishibekairohan 5:3ae504b88679 140
kishibekairohan 5:3ae504b88679 141 //************カラーセンサ変数********************
kishibekairohan 5:3ae504b88679 142
kishibekairohan 5:3ae504b88679 143 //************ライントレース変数*******************
kishibekairohan 5:3ae504b88679 144 int Point[3] = {234, 466, 590};//赤，緑，青
kishibekairohan 5:3ae504b88679 145 int colorSN;
kishibekairohan 5:3ae504b88679 146 int startP = 35;
kishibekairohan 5:3ae504b88679 147 int downP = 5;
kishibekairohan 5:3ae504b88679 148
kishibekairohan 5:3ae504b88679 149 bool Goal_flag = false;
kishibekairohan 5:3ae504b88679 150 PID goal = PID(0.03,-255,255,0,0,0);
kishibekairohan 5:3ae504b88679 151 void GoalArrival();
kishibekairohan 5:3ae504b88679 152 //************ライントレース変数*******************
kishibekairohan 5:3ae504b88679 153
kishibekairohan 5:3ae504b88679 154 //************ジャイロ*******************
kishibekairohan 5:3ae504b88679 155 //void AngleDetection();
kishibekairohan 5:3ae504b88679 156 //void AngleControl();
kishibekairohan 5:3ae504b88679 157 float AngleY;
kishibekairohan 5:3ae504b88679 158 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan 5:3ae504b88679 159 bool Angle_flag = false;
kishibekairohan 5:3ae504b88679 160 float rotateY;
kishibekairohan 5:3ae504b88679 161 int AngletargetX = 50;
kishibekairohan 5:3ae504b88679 162 int AngletargetY = -50;
kishibekairohan 5:3ae504b88679 163 int Angle_I;
kishibekairohan 5:3ae504b88679 164 //************ジャイロ*******************
kishibekairohan 5:3ae504b88679 165
kishibekairohan 5:3ae504b88679 166 //************ロタコン*******************
kishibekairohan 5:3ae504b88679 167 int MemoRt;
kishibekairohan 5:3ae504b88679 168 int Rt0;
kishibekairohan 5:3ae504b88679 169 int Rt1;
kishibekairohan 5:3ae504b88679 170 int Rt_A;
kishibekairohan 5:3ae504b88679 171 int Rt_B;
kishibekairohan 5:3ae504b88679 172 int PresentRt;
kishibekairohan 5:3ae504b88679 173 //************ロタコン*******************
t_yamamoto 0:669ef71cba68 174 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 175
t_yamamoto 0:669ef71cba68 176 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 177 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 178 static void Process0(void);
t_yamamoto 0:669ef71cba68 179 #endif
t_yamamoto 0:669ef71cba68 180 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 181 static void Process1(void);
t_yamamoto 0:669ef71cba68 182 #endif
t_yamamoto 0:669ef71cba68 183 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 184 static void Process2(void);
t_yamamoto 0:669ef71cba68 185 #endif
t_yamamoto 0:669ef71cba68 186 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 187 static void Process3(void);
t_yamamoto 0:669ef71cba68 188 #endif
t_yamamoto 0:669ef71cba68 189 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 190 static void Process4(void);
t_yamamoto 0:669ef71cba68 191 #endif
t_yamamoto 0:669ef71cba68 192 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 193 static void Process5(void);
t_yamamoto 0:669ef71cba68 194 #endif
t_yamamoto 0:669ef71cba68 195 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 196 static void Process6(void);
t_yamamoto 0:669ef71cba68 197 #endif
t_yamamoto 0:669ef71cba68 198 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 199 static void Process7(void);
t_yamamoto 0:669ef71cba68 200 #endif
t_yamamoto 0:669ef71cba68 201 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 202 static void Process8(void);
t_yamamoto 0:669ef71cba68 203 #endif
t_yamamoto 0:669ef71cba68 204 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 205 static void Process9(void);
t_yamamoto 0:669ef71cba68 206 #endif
t_yamamoto 0:669ef71cba68 207 #endif
t_yamamoto 0:669ef71cba68 208
t_yamamoto 0:669ef71cba68 209 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 210 {
t_yamamoto 0:669ef71cba68 211 #pragma region USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 212
t_yamamoto 0:669ef71cba68 213 /*Replace here with the initialization code of your variables.*/
t_yamamoto 0:669ef71cba68 214 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 215
t_yamamoto 0:669ef71cba68 216 lock = true;
t_yamamoto 0:669ef71cba68 217 processChangeComp = true;
t_yamamoto 0:669ef71cba68 218 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 219
t_yamamoto 0:669ef71cba68 220 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 221 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 222 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 223 #endif
t_yamamoto 0:669ef71cba68 224 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 225 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 226 #endif
t_yamamoto 0:669ef71cba68 227 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 228 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 229 #endif
t_yamamoto 0:669ef71cba68 230 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 231 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 232 #endif
t_yamamoto 0:669ef71cba68 233 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 234 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 235 #endif
t_yamamoto 0:669ef71cba68 236 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 237 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 238 #endif
t_yamamoto 0:669ef71cba68 239 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 240 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 241 #endif
t_yamamoto 0:669ef71cba68 242 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 243 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 244 #endif
t_yamamoto 0:669ef71cba68 245 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 246 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 247 #endif
t_yamamoto 0:669ef71cba68 248 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 249 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 250 #endif
t_yamamoto 0:669ef71cba68 251 #endif
t_yamamoto 0:669ef71cba68 252 }
t_yamamoto 0:669ef71cba68 253
t_yamamoto 0:669ef71cba68 254 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 255 {
t_yamamoto 0:669ef71cba68 256 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 257 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 258
t_yamamoto 0:669ef71cba68 259 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 260 {
t_yamamoto 0:669ef71cba68 261 current++;
t_yamamoto 0:669ef71cba68 262 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 263 processChangeComp = false;
t_yamamoto 0:669ef71cba68 264 }
t_yamamoto 0:669ef71cba68 265 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 266 {
t_yamamoto 0:669ef71cba68 267 current--;
t_yamamoto 0:669ef71cba68 268 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 269 processChangeComp = false;
t_yamamoto 0:669ef71cba68 270 }
t_yamamoto 0:669ef71cba68 271 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 272 #endif
t_yamamoto 0:669ef71cba68 273
t_yamamoto 0:669ef71cba68 274 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 275 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 276 #endif
t_yamamoto 0:669ef71cba68 277
t_yamamoto 0:669ef71cba68 278 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 279 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 280 #endif
t_yamamoto 0:669ef71cba68 281
t_yamamoto 0:669ef71cba68 282 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 283 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 284 #endif
t_yamamoto 0:669ef71cba68 285
t_yamamoto 0:669ef71cba68 286 }
t_yamamoto 0:669ef71cba68 287
kishibekairohan 2:c015739085d3 288
kishibekairohan 2:c015739085d3 289
t_yamamoto 0:669ef71cba68 290 void SystemProcess()
t_yamamoto 0:669ef71cba68 291 {
t_yamamoto 0:669ef71cba68 292 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 293
t_yamamoto 0:669ef71cba68 294 while(1)
kishibekairohan 5:3ae504b88679 295 {
kishibekairohan 2:c015739085d3 296
t_yamamoto 0:669ef71cba68 297 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 298 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 299 #endif
t_yamamoto 0:669ef71cba68 300
t_yamamoto 0:669ef71cba68 301 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 302 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 303 {
t_yamamoto 0:669ef71cba68 304 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 305 AllActuatorReset();
t_yamamoto 0:669ef71cba68 306 lock = true;
t_yamamoto 0:669ef71cba68 307 }
t_yamamoto 0:669ef71cba68 308 else
t_yamamoto 0:669ef71cba68 309 #endif
t_yamamoto 0:669ef71cba68 310 {
t_yamamoto 0:669ef71cba68 311
t_yamamoto 0:669ef71cba68 312 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 313 if(!lock)
t_yamamoto 0:669ef71cba68 314 {
t_yamamoto 0:669ef71cba68 315 Process[current]();
t_yamamoto 0:669ef71cba68 316 }
t_yamamoto 0:669ef71cba68 317 else
t_yamamoto 0:669ef71cba68 318 #endif
t_yamamoto 0:669ef71cba68 319 {
t_yamamoto 0:669ef71cba68 320 //ロック時の処理
t_yamamoto 0:669ef71cba68 321 }
t_yamamoto 0:669ef71cba68 322 }
t_yamamoto 0:669ef71cba68 323
t_yamamoto 0:669ef71cba68 324 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 325 }
t_yamamoto 0:669ef71cba68 326 }
t_yamamoto 0:669ef71cba68 327
kishibekairohan 2:c015739085d3 328
kishibekairohan 2:c015739085d3 329
kishibekairohan 2:c015739085d3 330
t_yamamoto 0:669ef71cba68 331 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 332 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 333 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 334 static void Process0()
t_yamamoto 0:669ef71cba68 335 {
kishibekairohan 5:3ae504b88679 336
kishibekairohan 5:3ae504b88679 337 if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
kishibekairohan 5:3ae504b88679 338 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 339 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 340 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 341 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 342 }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
kishibekairohan 5:3ae504b88679 343 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 344 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 345 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 346 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 347 }
kishibekairohan 5:3ae504b88679 348 for(int i = 0;i<20;i++){
kishibekairohan 5:3ae504b88679 349 float y = 0;
kishibekairohan 5:3ae504b88679 350 y = acc[1]*1000;
kishibekairohan 5:3ae504b88679 351 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 5:3ae504b88679 352 AngleY += rotateY;
kishibekairohan 5:3ae504b88679 353 }
kishibekairohan 5:3ae504b88679 354 AngleY = AngleY /20;
kishibekairohan 5:3ae504b88679 355 int gyropwm = gyro.SetPV(AngleY,Angle_I);
kishibekairohan 5:3ae504b88679 356
kishibekairohan 5:3ae504b88679 357 if(controller->Button.A){
kishibekairohan 5:3ae504b88679 358 Angle_flag = true;
kishibekairohan 5:3ae504b88679 359 }/*
kishibekairohan 5:3ae504b88679 360 if(controller->Button.Y){
kishibekairohan 5:3ae504b88679 361 Angle_flag = true;
kishibekairohan 5:3ae504b88679 362 }*/
kishibekairohan 5:3ae504b88679 363 if (Angle_flag){
kishibekairohan 5:3ae504b88679 364 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 5:3ae504b88679 365 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 5:3ae504b88679 366 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 367 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 368 if(Angle_I - 2 < AngleY && AngleY < Angle_I + 2){
kishibekairohan 5:3ae504b88679 369 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 370 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 371 Angle_flag = false;
kishibekairohan 5:3ae504b88679 372 }
kishibekairohan 5:3ae504b88679 373 }
t_yamamoto 0:669ef71cba68 374 }
t_yamamoto 0:669ef71cba68 375 #endif
t_yamamoto 0:669ef71cba68 376
t_yamamoto 0:669ef71cba68 377 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 378 static void Process1()
t_yamamoto 0:669ef71cba68 379 {
kishibekairohan 5:3ae504b88679 380 motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
kishibekairohan 5:3ae504b88679 381 motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
kishibekairohan 5:3ae504b88679 382 motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 5:3ae504b88679 383 motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 2:c015739085d3 384
kishibekairohan 5:3ae504b88679 385 motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
kishibekairohan 5:3ae504b88679 386 motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
kishibekairohan 5:3ae504b88679 387 motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
kishibekairohan 5:3ae504b88679 388 motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
kishibekairohan 2:c015739085d3 389
kishibekairohan 2:c015739085d3 390 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 5:3ae504b88679 391 motor[TIRE_FR].pwm = motor[0].pwm * 1.3;
kishibekairohan 5:3ae504b88679 392 motor[TIRE_FL].pwm = motor[1].pwm * 1.3;
kishibekairohan 2:c015739085d3 393 }
t_yamamoto 0:669ef71cba68 394 }
t_yamamoto 0:669ef71cba68 395 #endif
t_yamamoto 0:669ef71cba68 396
t_yamamoto 0:669ef71cba68 397 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 398 static void Process2()
t_yamamoto 0:669ef71cba68 399 {
kishibekairohan 5:3ae504b88679 400 static bool color_flag = false;
kishibekairohan 5:3ae504b88679 401 static bool traceon = false;
kishibekairohan 5:3ae504b88679 402
kishibekairohan 5:3ae504b88679 403 static bool yokofla = false;
kishibekairohan 2:c015739085d3 404
kishibekairohan 5:3ae504b88679 405 static bool compA = false;
kishibekairohan 5:3ae504b88679 406 static bool compB = false;
kishibekairohan 5:3ae504b88679 407 static bool compC = false;
kishibekairohan 5:3ae504b88679 408 static bool compD = false;
kishibekairohan 5:3ae504b88679 409
kishibekairohan 5:3ae504b88679 410 static bool invationA = false;
kishibekairohan 5:3ae504b88679 411 static bool invationB = false;
kishibekairohan 5:3ae504b88679 412 static bool invationC = false;
kishibekairohan 5:3ae504b88679 413 static bool invationD = false;
kishibekairohan 5:3ae504b88679 414
kishibekairohan 5:3ae504b88679 415 ColorDetection();
kishibekairohan 5:3ae504b88679 416 //
kishibekairohan 2:c015739085d3 417 if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 2:c015739085d3 418 {
kishibekairohan 2:c015739085d3 419 invationA ^= 1;//start false,over true
kishibekairohan 2:c015739085d3 420 compA = true;//on true,noon false
kishibekairohan 2:c015739085d3 421 }
kishibekairohan 2:c015739085d3 422 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
kishibekairohan 2:c015739085d3 423
kishibekairohan 5:3ae504b88679 424 if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
kishibekairohan 2:c015739085d3 425 {
kishibekairohan 5:3ae504b88679 426 invationB ^= 1;//start false,over true
kishibekairohan 5:3ae504b88679 427 compB = true;//on true,noon false
kishibekairohan 5:3ae504b88679 428 }
kishibekairohan 5:3ae504b88679 429 else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
kishibekairohan 5:3ae504b88679 430 //
kishibekairohan 5:3ae504b88679 431
kishibekairohan 5:3ae504b88679 432 if(controller->Button.B && !color_flag)
kishibekairohan 5:3ae504b88679 433 {
kishibekairohan 5:3ae504b88679 434 traceon ^= 1;
kishibekairohan 5:3ae504b88679 435 color_flag = true;
kishibekairohan 5:3ae504b88679 436 }
kishibekairohan 5:3ae504b88679 437 else if(!controller->Button.B)
kishibekairohan 5:3ae504b88679 438 {
kishibekairohan 5:3ae504b88679 439 color_flag = false;
kishibekairohan 2:c015739085d3 440 }
kishibekairohan 2:c015739085d3 441
kishibekairohan 5:3ae504b88679 442 if(traceon)
kishibekairohan 2:c015739085d3 443 {
kishibekairohan 5:3ae504b88679 444 if(!invationA && !compA && !invationB && !compB){
kishibekairohan 5:3ae504b88679 445 motor[TIRE_FR].dir = FOR;
kishibekairohan 5:3ae504b88679 446 motor[TIRE_FL].dir = FOR;
kishibekairohan 5:3ae504b88679 447 motor[TIRE_BR].dir = BACK;
kishibekairohan 5:3ae504b88679 448 motor[TIRE_BL].dir = BACK;
kishibekairohan 5:3ae504b88679 449
kishibekairohan 5:3ae504b88679 450 motor[TIRE_FR].pwm = startP;
kishibekairohan 5:3ae504b88679 451 motor[TIRE_FL].pwm = startP;
kishibekairohan 5:3ae504b88679 452 motor[TIRE_BR].pwm = startP;
kishibekairohan 5:3ae504b88679 453 motor[TIRE_BL].pwm = startP;
kishibekairohan 5:3ae504b88679 454 }
kishibekairohan 5:3ae504b88679 455 else if(invationA && compA && !invationB && !compB){
kishibekairohan 5:3ae504b88679 456 motor[TIRE_FR].dir = FOR;
kishibekairohan 5:3ae504b88679 457 motor[TIRE_FL].dir = FOR;
kishibekairohan 5:3ae504b88679 458 motor[TIRE_BR].dir = BACK;
kishibekairohan 5:3ae504b88679 459 motor[TIRE_BL].dir = BACK;
kishibekairohan 5:3ae504b88679 460
kishibekairohan 5:3ae504b88679 461 motor[TIRE_FR].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 462 motor[TIRE_FL].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 463 motor[TIRE_BR].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 464 motor[TIRE_BL].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 465 }
kishibekairohan 5:3ae504b88679 466 else if(!invationA && !compA && !invationB && !compB){
kishibekairohan 5:3ae504b88679 467 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 468 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 469 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 470 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 471
kishibekairohan 5:3ae504b88679 472 motor[0].pwm = 255;
kishibekairohan 5:3ae504b88679 473 motor[1].pwm = 255;
kishibekairohan 5:3ae504b88679 474 motor[2].pwm = 255;
kishibekairohan 5:3ae504b88679 475 motor[3].pwm = 255;
kishibekairohan 5:3ae504b88679 476
kishibekairohan 5:3ae504b88679 477 wait(5);
kishibekairohan 5:3ae504b88679 478 yokofla = true;
kishibekairohan 5:3ae504b88679 479 }
kishibekairohan 5:3ae504b88679 480 else if(invationA && !compA && invationB && !compB && yokofla){
kishibekairohan 5:3ae504b88679 481 motor[TIRE_FR].dir = BACK;
kishibekairohan 5:3ae504b88679 482 motor[TIRE_FL].dir = BACK;
kishibekairohan 5:3ae504b88679 483 motor[TIRE_BR].dir = FOR;
kishibekairohan 5:3ae504b88679 484 motor[TIRE_BL].dir = FOR;
kishibekairohan 5:3ae504b88679 485
kishibekairohan 5:3ae504b88679 486 motor[TIRE_FR].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 487 motor[TIRE_FL].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 488 motor[TIRE_BR].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 489 motor[TIRE_BL].pwm = startP - downP;
kishibekairohan 5:3ae504b88679 490 }
kishibekairohan 5:3ae504b88679 491 else if(invationA && !compA && !invationB && !compB && yokofla){
kishibekairohan 5:3ae504b88679 492 motor[TIRE_FR].dir = FOR;
kishibekairohan 5:3ae504b88679 493 motor[TIRE_FL].dir = BACK;
kishibekairohan 5:3ae504b88679 494 motor[TIRE_BR].dir = FOR;
kishibekairohan 5:3ae504b88679 495 motor[TIRE_BL].dir = BACK;
kishibekairohan 2:c015739085d3 496
kishibekairohan 5:3ae504b88679 497 motor[TIRE_FR].pwm = startP;
kishibekairohan 5:3ae504b88679 498 motor[TIRE_FL].pwm = startP;
kishibekairohan 5:3ae504b88679 499 motor[TIRE_BR].pwm = startP;
kishibekairohan 5:3ae504b88679 500 motor[TIRE_BL].pwm = startP;
kishibekairohan 5:3ae504b88679 501 }
kishibekairohan 5:3ae504b88679 502 else if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4) && invationA && !compA && !invationB && !compB && yokofla && traceon){
kishibekairohan 5:3ae504b88679 503 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 504 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 505 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 506 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 507
kishibekairohan 5:3ae504b88679 508 motor[TIRE_FR].pwm = 255;
kishibekairohan 5:3ae504b88679 509 motor[TIRE_FL].pwm = 255;
kishibekairohan 5:3ae504b88679 510 motor[TIRE_BR].pwm = 255;
kishibekairohan 5:3ae504b88679 511 motor[TIRE_BL].pwm = 255;
kishibekairohan 5:3ae504b88679 512 }
kishibekairohan 5:3ae504b88679 513 /*////
kishibekairohan 5:3ae504b88679 514 motor[0].dir = BACK;
kishibekairohan 5:3ae504b88679 515 motor[1].dir = BACK;
kishibekairohan 5:3ae504b88679 516 motor[2].dir = FOR;
kishibekairohan 5:3ae504b88679 517 motor[3].dir = FOR;
kishibekairohan 5:3ae504b88679 518
kishibekairohan 5:3ae504b88679 519 motor[0].pwm = startP;
kishibekairohan 5:3ae504b88679 520 motor[1].pwm = startP;
kishibekairohan 5:3ae504b88679 521 motor[2].pwm = startP;
kishibekairohan 5:3ae504b88679 522 motor[3].pwm = startP;
kishibekairohan 5:3ae504b88679 523 else if()
kishibekairohan 5:3ae504b88679 524 {
kishibekairohan 5:3ae504b88679 525 motor[0].dir = BRAKE;
kishibekairohan 5:3ae504b88679 526 motor[1].dir = BRAKE;
kishibekairohan 5:3ae504b88679 527 motor[2].dir = BRAKE;
kishibekairohan 5:3ae504b88679 528 motor[3].dir = BRAKE;
kishibekairohan 5:3ae504b88679 529
kishibekairohan 5:3ae504b88679 530 motor[0].pwm = 255;
kishibekairohan 5:3ae504b88679 531 motor[1].pwm = 255;
kishibekairohan 5:3ae504b88679 532 motor[2].pwm = 255;
kishibekairohan 5:3ae504b88679 533 motor[3].pwm = 255;
kishibekairohan 5:3ae504b88679 534 }*//////
kishibekairohan 5:3ae504b88679 535 else if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)){
kishibekairohan 5:3ae504b88679 536 motor[TIRE_FR].dir = FOR;
kishibekairohan 5:3ae504b88679 537 motor[TIRE_FL].dir = FOR;
kishibekairohan 5:3ae504b88679 538 motor[TIRE_BR].dir = BACK;
kishibekairohan 5:3ae504b88679 539 motor[TIRE_BL].dir = BACK;
kishibekairohan 5:3ae504b88679 540 motor[TIRE_FR].pwm = 100;
kishibekairohan 5:3ae504b88679 541 motor[TIRE_FL].pwm = 100;
kishibekairohan 5:3ae504b88679 542 motor[TIRE_BR].pwm = 100;
kishibekairohan 5:3ae504b88679 543 motor[TIRE_BL].pwm = 100;
kishibekairohan 5:3ae504b88679 544 }else if(!LimitSw::IsPressed(3) && LimitSw::IsPressed(4)){
kishibekairohan 5:3ae504b88679 545 motor[TIRE_FR].dir = FOR;
kishibekairohan 5:3ae504b88679 546 motor[TIRE_BR].dir = FOR;
kishibekairohan 5:3ae504b88679 547 motor[TIRE_FR].pwm = 20;
kishibekairohan 5:3ae504b88679 548 motor[TIRE_BR].pwm = 20;
kishibekairohan 5:3ae504b88679 549 }else if(LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
kishibekairohan 5:3ae504b88679 550 motor[TIRE_FL].dir = BACK;
kishibekairohan 5:3ae504b88679 551 motor[TIRE_BL].dir = BACK;
kishibekairohan 5:3ae504b88679 552 motor[TIRE_FL].pwm = 20;
kishibekairohan 5:3ae504b88679 553 motor[TIRE_BL].pwm = 20;
kishibekairohan 5:3ae504b88679 554 }
kishibekairohan 5:3ae504b88679 555 else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
kishibekairohan 5:3ae504b88679 556 Goal_flag = true;
kishibekairohan 5:3ae504b88679 557 GoalArrival();
kishibekairohan 5:3ae504b88679 558 }else{
kishibekairohan 5:3ae504b88679 559 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 560 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 561 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 562 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 563 }
kishibekairohan 5:3ae504b88679 564 }
t_yamamoto 0:669ef71cba68 565 }
t_yamamoto 0:669ef71cba68 566 #endif
t_yamamoto 0:669ef71cba68 567
t_yamamoto 0:669ef71cba68 568 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 569 static void Process3()
t_yamamoto 0:669ef71cba68 570 {
kishibekairohan 2:c015739085d3 571 if(controller->Button.R){
kishibekairohan 5:3ae504b88679 572 motor[Angle_R].dir = FOR;
kishibekairohan 5:3ae504b88679 573 motor[Angle_L].dir = BACK;
kishibekairohan 5:3ae504b88679 574 motor[Angle_R].pwm = 150;
kishibekairohan 5:3ae504b88679 575 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 576 }else if(controller->Button.L){
kishibekairohan 5:3ae504b88679 577 motor[Angle_R].dir = BACK;
kishibekairohan 5:3ae504b88679 578 motor[Angle_L].dir = FOR;
kishibekairohan 5:3ae504b88679 579 motor[Angle_R].pwm = 150;
kishibekairohan 5:3ae504b88679 580 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 581 }else{
kishibekairohan 5:3ae504b88679 582 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 583 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 584 }
kishibekairohan 5:3ae504b88679 585
kishibekairohan 5:3ae504b88679 586 if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
kishibekairohan 5:3ae504b88679 587 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 588 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 589
kishibekairohan 5:3ae504b88679 590 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 591 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 592 }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
kishibekairohan 5:3ae504b88679 593 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 594 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 595
kishibekairohan 5:3ae504b88679 596 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 597 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 598 }
t_yamamoto 0:669ef71cba68 599 }
t_yamamoto 0:669ef71cba68 600 #endif
t_yamamoto 0:669ef71cba68 601
t_yamamoto 0:669ef71cba68 602 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 603 static void Process4()
t_yamamoto 0:669ef71cba68 604 {
kishibekairohan 5:3ae504b88679 605 /* for(int i=0;i<10;i++)
kishibekairohan 2:c015739085d3 606 {
kishibekairohan 2:c015739085d3 607 ColorDetection();
kishibekairohan 2:c015739085d3 608
kishibekairohan 5:3ae504b88679 609 averageR_A += Color_A[0];
kishibekairohan 5:3ae504b88679 610 averageG_A += Color_A[1];
kishibekairohan 5:3ae504b88679 611 averageB_A += Color_A[2];
kishibekairohan 5:3ae504b88679 612 averageR_B += Color_B[0];
kishibekairohan 5:3ae504b88679 613 averageG_B += Color_B[1];
kishibekairohan 5:3ae504b88679 614 averageB_B += Color_B[2];
kishibekairohan 5:3ae504b88679 615 averageR_C += Color_C[0];
kishibekairohan 5:3ae504b88679 616 averageG_C += Color_C[1];
kishibekairohan 5:3ae504b88679 617 averageB_C += Color_C[2];
kishibekairohan 5:3ae504b88679 618 averageR_D += Color_D[0];
kishibekairohan 5:3ae504b88679 619 averageG_D += Color_D[1];
kishibekairohan 5:3ae504b88679 620 averageB_D += Color_D[2];
kishibekairohan 2:c015739085d3 621 }
kishibekairohan 5:3ae504b88679 622 pc.printf("AR_A:%d, AG_A:%d ,AB_A:%d \r\n",averageR_A / 10 ,averageG_A / 10, averageB_A / 10);
kishibekairohan 5:3ae504b88679 623 pc.printf("AR_B:%d, AG_B:%d ,AB_B:%d \r\n",averageR_B / 10 ,averageG_B / 10, averageB_B / 10);
kishibekairohan 5:3ae504b88679 624 pc.printf("AR_C:%d, AG_C:%d ,AB_C:%d \r\n",averageR_C / 10 ,averageG_C / 10, averageB_C / 10);
kishibekairohan 5:3ae504b88679 625 pc.printf("AR_D:%d, AG_D:%d ,AB_D:%d \r\n",averageR_D / 10 ,averageG_D / 10, averageB_D / 10);
kishibekairohan 2:c015739085d3 626
kishibekairohan 5:3ae504b88679 627 averageR_A = 0;
kishibekairohan 5:3ae504b88679 628 averageG_A = 0;
kishibekairohan 5:3ae504b88679 629 averageB_A = 0;
kishibekairohan 5:3ae504b88679 630 averageR_B = 0;
kishibekairohan 5:3ae504b88679 631 averageG_B = 0;
kishibekairohan 5:3ae504b88679 632 averageB_B = 0;
kishibekairohan 5:3ae504b88679 633 averageR_C = 0;
kishibekairohan 5:3ae504b88679 634 averageG_C = 0;
kishibekairohan 5:3ae504b88679 635 averageB_C = 0;
kishibekairohan 5:3ae504b88679 636 averageR_D = 0;
kishibekairohan 5:3ae504b88679 637 averageG_D = 0;
kishibekairohan 5:3ae504b88679 638 averageB_D = 0;*/
t_yamamoto 0:669ef71cba68 639 }
t_yamamoto 0:669ef71cba68 640 #endif
t_yamamoto 0:669ef71cba68 641
t_yamamoto 0:669ef71cba68 642 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 643 static void Process5()
kishibekairohan 5:3ae504b88679 644 {
kishibekairohan 5:3ae504b88679 645 if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
kishibekairohan 5:3ae504b88679 646 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 647 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 648 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 649 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 650 }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
kishibekairohan 5:3ae504b88679 651 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 652 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 653 motor[Angle_R].pwm = 255;
kishibekairohan 5:3ae504b88679 654 motor[Angle_L].pwm = 255;
kishibekairohan 5:3ae504b88679 655 }
kishibekairohan 5:3ae504b88679 656 for(int i = 0;i<20;i++){
kishibekairohan 5:3ae504b88679 657 float y = 0;
kishibekairohan 5:3ae504b88679 658 y = acc[1]*1000;
kishibekairohan 5:3ae504b88679 659 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 5:3ae504b88679 660 AngleY += rotateY;
kishibekairohan 5:3ae504b88679 661 }
kishibekairohan 5:3ae504b88679 662 AngleY = AngleY /20;
kishibekairohan 5:3ae504b88679 663 int gyropwm = gyro.SetPV(AngleY,AngletargetY);
kishibekairohan 5:3ae504b88679 664
kishibekairohan 5:3ae504b88679 665 if(controller->Button.X){
kishibekairohan 5:3ae504b88679 666 Angle_flag = true;
kishibekairohan 5:3ae504b88679 667 }/*
kishibekairohan 5:3ae504b88679 668 if(controller->Button.Y){
kishibekairohan 5:3ae504b88679 669 Angle_flag = true;
kishibekairohan 5:3ae504b88679 670 }*/
kishibekairohan 5:3ae504b88679 671 if (Angle_flag){
kishibekairohan 5:3ae504b88679 672 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 5:3ae504b88679 673 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 5:3ae504b88679 674 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 675 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 676 if(AngletargetY - 2 < AngleY && AngleY < AngletargetY + 2){
kishibekairohan 5:3ae504b88679 677 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 678 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 679 Angle_flag = false;
kishibekairohan 5:3ae504b88679 680 }
kishibekairohan 5:3ae504b88679 681 }
kishibekairohan 5:3ae504b88679 682 /*float y = 0;
kishibekairohan 5:3ae504b88679 683 y = acc[1]*1000;
kishibekairohan 5:3ae504b88679 684 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 5:3ae504b88679 685 int gyropwm = gyro.SetPV(rotateY , Angletarget);
kishibekairohan 5:3ae504b88679 686
kishibekairohan 5:3ae504b88679 687 if(controller->Button.X){
kishibekairohan 5:3ae504b88679 688 Angle_flag = true;
kishibekairohan 5:3ae504b88679 689 }
kishibekairohan 5:3ae504b88679 690 if (Angle_flag){
kishibekairohan 5:3ae504b88679 691 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 5:3ae504b88679 692 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 5:3ae504b88679 693 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 694 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 695 if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
kishibekairohan 5:3ae504b88679 696 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 697 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 698 Angle_flag = false;
kishibekairohan 5:3ae504b88679 699 }
kishibekairohan 5:3ae504b88679 700 }*/
kishibekairohan 5:3ae504b88679 701 else{
kishibekairohan 5:3ae504b88679 702 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 703 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 704 }
t_yamamoto 0:669ef71cba68 705 }
t_yamamoto 0:669ef71cba68 706 #endif
t_yamamoto 0:669ef71cba68 707
t_yamamoto 0:669ef71cba68 708 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 709 static void Process6()
kishibekairohan 5:3ae504b88679 710 {
kishibekairohan 5:3ae504b88679 711 //void Int::Initialize(BoardRtInt[0].fall(int2));
t_yamamoto 0:669ef71cba68 712 }
t_yamamoto 0:669ef71cba68 713 #endif
t_yamamoto 0:669ef71cba68 714
t_yamamoto 0:669ef71cba68 715 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 716 static void Process7()
t_yamamoto 0:669ef71cba68 717 {
t_yamamoto 0:669ef71cba68 718
t_yamamoto 0:669ef71cba68 719 }
t_yamamoto 0:669ef71cba68 720 #endif
t_yamamoto 0:669ef71cba68 721
t_yamamoto 0:669ef71cba68 722 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 723 static void Process8()
t_yamamoto 0:669ef71cba68 724 {
t_yamamoto 0:669ef71cba68 725
t_yamamoto 0:669ef71cba68 726 }
t_yamamoto 0:669ef71cba68 727 #endif
t_yamamoto 0:669ef71cba68 728
t_yamamoto 0:669ef71cba68 729 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 730 static void Process9()
t_yamamoto 0:669ef71cba68 731 {
t_yamamoto 0:669ef71cba68 732
t_yamamoto 0:669ef71cba68 733 }
t_yamamoto 0:669ef71cba68 734 #endif
t_yamamoto 0:669ef71cba68 735 #endif
t_yamamoto 0:669ef71cba68 736 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 737
t_yamamoto 0:669ef71cba68 738 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 739 {
t_yamamoto 0:669ef71cba68 740
t_yamamoto 0:669ef71cba68 741 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 742 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 743 #endif
t_yamamoto 0:669ef71cba68 744
t_yamamoto 0:669ef71cba68 745 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 746 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 747 {
t_yamamoto 0:669ef71cba68 748 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 749 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 750 }
t_yamamoto 0:669ef71cba68 751 #endif
t_yamamoto 0:669ef71cba68 752 }
t_yamamoto 0:669ef71cba68 753
t_yamamoto 0:669ef71cba68 754 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 2:c015739085d3 755 void ColorDetection(){
kishibekairohan 2:c015739085d3 756 GATE = 0;
kishibekairohan 2:c015739085d3 757
kishibekairohan 2:c015739085d3 758 CK[0] = 0;
kishibekairohan 2:c015739085d3 759 CK[1] = 0;
kishibekairohan 2:c015739085d3 760 CK[2] = 0;
kishibekairohan 2:c015739085d3 761 CK[3] = 0;
kishibekairohan 2:c015739085d3 762
kishibekairohan 2:c015739085d3 763 RANGE = 1;
kishibekairohan 2:c015739085d3 764
kishibekairohan 2:c015739085d3 765 GATE = 1;
kishibekairohan 2:c015739085d3 766 wait_ms(intergration);
kishibekairohan 2:c015739085d3 767 GATE = 0;
kishibekairohan 2:c015739085d3 768 wait_us(4);
kishibekairohan 2:c015739085d3 769
kishibekairohan 2:c015739085d3 770 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 771 wait_us(3);
kishibekairohan 2:c015739085d3 772 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 773 wait_us(3);
kishibekairohan 2:c015739085d3 774 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 775
kishibekairohan 2:c015739085d3 776 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 777 wait_us(3);
kishibekairohan 2:c015739085d3 778 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 779 wait_us(3);
kishibekairohan 2:c015739085d3 780 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 781
kishibekairohan 2:c015739085d3 782 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 783 wait_us(3);
kishibekairohan 2:c015739085d3 784 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 785 wait_us(3);
kishibekairohan 2:c015739085d3 786 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 787
kishibekairohan 2:c015739085d3 788 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 789 wait_us(3);
kishibekairohan 2:c015739085d3 790 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 791 wait_us(3);
kishibekairohan 2:c015739085d3 792 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 793 }
kishibekairohan 5:3ae504b88679 794 /*void AngleDetection(){
kishibekairohan 5:3ae504b88679 795 float x = 0, y= 0, z = 0;
kishibekairohan 5:3ae504b88679 796
kishibekairohan 5:3ae504b88679 797 //x = acc[0]*1000;
kishibekairohan 5:3ae504b88679 798 y = acc[1]*1000;
kishibekairohan 5:3ae504b88679 799 //z = acc[2]*1000;
kishibekairohan 5:3ae504b88679 800 //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
kishibekairohan 5:3ae504b88679 801 //float rotateX = (x - 306)/2.22 - 90;
kishibekairohan 5:3ae504b88679 802 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 5:3ae504b88679 803 //float rotateZ = (z - 300)/1.18 - 90;
kishibekairohan 5:3ae504b88679 804 //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
kishibekairohan 5:3ae504b88679 805 }*/
kishibekairohan 5:3ae504b88679 806 /*void AngleControl(){
kishibekairohan 5:3ae504b88679 807 int Angletarget = -50;
kishibekairohan 5:3ae504b88679 808 float AnglevalueY = rotateY;
kishibekairohan 5:3ae504b88679 809 int gyropwm = gyro.SetPV(AnglevalueY , Angletarget);
kishibekairohan 5:3ae504b88679 810
kishibekairohan 5:3ae504b88679 811 if (Angle_flag){
kishibekairohan 5:3ae504b88679 812 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 5:3ae504b88679 813 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 5:3ae504b88679 814 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 815 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 5:3ae504b88679 816 if(Angletarget - 2 < AnglevalueY && AnglevalueY < Angletarget + 2){
kishibekairohan 5:3ae504b88679 817 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 818 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 819 Angle_flag = false;
kishibekairohan 5:3ae504b88679 820 }
kishibekairohan 5:3ae504b88679 821 }else{
kishibekairohan 5:3ae504b88679 822 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 823 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 824 }
kishibekairohan 5:3ae504b88679 825 //pc.printf("PWM:%d \r\n" , gyropwm);
kishibekairohan 5:3ae504b88679 826 }*/
kishibekairohan 5:3ae504b88679 827 void GoalArrival(){
kishibekairohan 5:3ae504b88679 828 int Goaltarget = 0;
kishibekairohan 5:3ae504b88679 829 float Goalvalue = 0;
kishibekairohan 5:3ae504b88679 830 int goalpwm = goal.SetPV(Goalvalue,Goaltarget);
kishibekairohan 5:3ae504b88679 831
kishibekairohan 5:3ae504b88679 832 if (Goal_flag){
kishibekairohan 5:3ae504b88679 833 motor[TIRE_FR].dir = SetStatus(goalpwm);
kishibekairohan 5:3ae504b88679 834 motor[TIRE_FL].dir = SetStatus(-goalpwm);
kishibekairohan 5:3ae504b88679 835 motor[TIRE_BR].dir = SetStatus(goalpwm);
kishibekairohan 5:3ae504b88679 836 motor[TIRE_BL].dir = SetStatus(-goalpwm);
kishibekairohan 5:3ae504b88679 837 motor[TIRE_FR].pwm = SetPWM(goalpwm);
kishibekairohan 5:3ae504b88679 838 motor[TIRE_FL].pwm = SetPWM(goalpwm);
kishibekairohan 5:3ae504b88679 839 motor[TIRE_BR].pwm = SetPWM(goalpwm);
kishibekairohan 5:3ae504b88679 840 motor[TIRE_BL].pwm = SetPWM(goalpwm);
kishibekairohan 5:3ae504b88679 841 if(Goaltarget > Goalvalue - 10 && Goaltarget < Goalvalue+ 10){
kishibekairohan 5:3ae504b88679 842 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 843 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 844 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 5:3ae504b88679 845 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 5:3ae504b88679 846 Goal_flag = false;
kishibekairohan 5:3ae504b88679 847 }
kishibekairohan 5:3ae504b88679 848 }else{
kishibekairohan 5:3ae504b88679 849 motor[Angle_R].dir = BRAKE;
kishibekairohan 5:3ae504b88679 850 motor[Angle_L].dir = BRAKE;
kishibekairohan 5:3ae504b88679 851 }
kishibekairohan 5:3ae504b88679 852 }
kishibekairohan 5:3ae504b88679 853 /*
kishibekairohan 5:3ae504b88679 854 void RotaryD(uint8_t, uint8_t){
kishibekairohan 5:3ae504b88679 855 uint8_t D;
kishibekairohan 5:3ae504b88679 856 MemoRt = Rt1;
kishibekairohan 5:3ae504b88679 857 Rt0 = Rt0 << 1;
kishibekairohan 5:3ae504b88679 858 Rt0 = Rt0 & 3;
kishibekairohan 5:3ae504b88679 859 Rt1 = Rt1 & 3;
kishibekairohan 5:3ae504b88679 860 D = (Rt0 + Rt1) & 3;
kishibekairohan 5:3ae504b88679 861 //return (D);
kishibekairohan 5:3ae504b88679 862 }
kishibekairohan 5:3ae504b88679 863 void GetRt(){
kishibekairohan 5:3ae504b88679 864 uint8_t DJ;
kishibekairohan 5:3ae504b88679 865 double rad = 0;
kishibekairohan 5:3ae504b88679 866 PresentRt = 0x00;
kishibekairohan 5:3ae504b88679 867 if(LimitSw::IsPressed(Rt_A))
kishibekairohan 5:3ae504b88679 868 PresentRt = 0x02;
kishibekairohan 5:3ae504b88679 869 if(LimitSw::IsPressed(Rt_B))
kishibekairohan 5:3ae504b88679 870 PresentRt = 0x01;
kishibekairohan 5:3ae504b88679 871 if(MemoRt != PresentRt)
kishibekairohan 5:3ae504b88679 872 DJ = RotaryD(MemoRt,PresentRt);
kishibekairohan 5:3ae504b88679 873 if(D>=2)rad = 360.0 /50.0;
kishibekairohan 5:3ae504b88679 874 else rad = -(360.0/50.0);
kishibekairohan 5:3ae504b88679 875 Rt = Rt + rad /360.0*20;
kishibekairohan 5:3ae504b88679 876 }*/
kishibekairohan 5:3ae504b88679 877
t_yamamoto 0:669ef71cba68 878 #pragma endregion