大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_18

daad

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@4:ba9df71868df, 2018-10-01 (annotated)

Committer:
7ka884
Date:
Mon Oct 01 13:47:19 2018 +0000
Revision:
4:ba9df71868df
Parent:
3:e10d8736fd22
Child:
6:10e22bc327ce
huhuhuh

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