大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_17

daad

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@2:c015739085d3, 2018-09-22 (annotated)

Committer:
kishibekairohan
Date:
Sat Sep 22 10:55:22 2018 +0000
Revision:
2:c015739085d3
Parent:
1:b1219d8ca117
Child:
3:e10d8736fd22
naka

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"
t_yamamoto 0:669ef71cba68 15
kishibekairohan 2:c015739085d3 16
t_yamamoto 0:669ef71cba68 17 using namespace SWITCH;
kishibekairohan 2:c015739085d3 18 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 19 using namespace ACCELERATIONSENSOR;
t_yamamoto 0:669ef71cba68 20
t_yamamoto 0:669ef71cba68 21 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 22 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 23 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 24
t_yamamoto 0:669ef71cba68 25 static bool lock;
t_yamamoto 0:669ef71cba68 26 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 27 static int current;
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 32 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 33 #endif
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 38
kishibekairohan 2:c015739085d3 39 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 40
kishibekairohan 2:c015739085d3 41 unsigned long ColorIn(int index)
kishibekairohan 2:c015739085d3 42 {
kishibekairohan 2:c015739085d3 43 int result = 0;
kishibekairohan 2:c015739085d3 44 bool rtn = false;
kishibekairohan 2:c015739085d3 45 for(int i=0; i<12; i++)
kishibekairohan 2:c015739085d3 46 {
kishibekairohan 2:c015739085d3 47 CK[index] = 1;
kishibekairohan 2:c015739085d3 48 rtn = DOUT[index];
kishibekairohan 2:c015739085d3 49 CK[index] = 0;
kishibekairohan 2:c015739085d3 50 if(rtn)
kishibekairohan 2:c015739085d3 51 {
kishibekairohan 2:c015739085d3 52 result|=(1 << i);
kishibekairohan 2:c015739085d3 53 }
kishibekairohan 2:c015739085d3 54 }
kishibekairohan 2:c015739085d3 55 return result;
kishibekairohan 2:c015739085d3 56 }
kishibekairohan 2:c015739085d3 57
kishibekairohan 2:c015739085d3 58 #define TILE_FR 0 //足回り前右
kishibekairohan 2:c015739085d3 59 #define TILE_FL 1 //足回り前左
kishibekairohan 2:c015739085d3 60 #define TILE_BR 2 //足回り後右
kishibekairohan 2:c015739085d3 61 #define TILE_BL 3 //足回り後左
kishibekairohan 2:c015739085d3 62
kishibekairohan 2:c015739085d3 63 #define Anguladjust_R 4 //角度調節右
kishibekairohan 2:c015739085d3 64 #define Anguladjust_L 5 //角度調節左
kishibekairohan 2:c015739085d3 65
kishibekairohan 2:c015739085d3 66 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 67 {
kishibekairohan 2:c015739085d3 68 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 69 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 70 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 71 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 72 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 73 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 74 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 75 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 76 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 77 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 78 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 79 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 80 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 81 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 82 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 83 };
kishibekairohan 2:c015739085d3 84
kishibekairohan 2:c015739085d3 85 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 86 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 87 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 88 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 89 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 90 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 91 else return BRAKE;
kishibekairohan 2:c015739085d3 92 }
kishibekairohan 2:c015739085d3 93 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 94 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 95 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 96 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 97 }
kishibekairohan 2:c015739085d3 98
kishibekairohan 2:c015739085d3 99 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 100 int Color_B[3];
kishibekairohan 2:c015739085d3 101 int Color_C[3];
kishibekairohan 2:c015739085d3 102 int Color_D[3];
kishibekairohan 2:c015739085d3 103 int intergration = 50;
kishibekairohan 2:c015739085d3 104
kishibekairohan 2:c015739085d3 105 //************ライントレース変数*******************
kishibekairohan 2:c015739085d3 106 int Point[3] = {234, 466, 590};//赤，緑，青
kishibekairohan 2:c015739085d3 107
kishibekairohan 2:c015739085d3 108 int startP = 150;
kishibekairohan 2:c015739085d3 109 int downP = 70;
kishibekairohan 2:c015739085d3 110
kishibekairohan 2:c015739085d3 111 bool compA = false;
kishibekairohan 2:c015739085d3 112 bool compB = false;
kishibekairohan 2:c015739085d3 113 bool compC = false;
kishibekairohan 2:c015739085d3 114 bool compD = false;
kishibekairohan 2:c015739085d3 115
kishibekairohan 2:c015739085d3 116 bool invationA = false;
kishibekairohan 2:c015739085d3 117 bool invationB = false;
kishibekairohan 2:c015739085d3 118 bool invationC = false;
kishibekairohan 2:c015739085d3 119 bool invationD = false;
kishibekairohan 2:c015739085d3 120 //************ライントレース変数*******************
kishibekairohan 2:c015739085d3 121
kishibekairohan 2:c015739085d3 122 int averageR_0;
kishibekairohan 2:c015739085d3 123 int averageG_0;
kishibekairohan 2:c015739085d3 124 int averageB_0;
kishibekairohan 2:c015739085d3 125 int averageR_1;
kishibekairohan 2:c015739085d3 126 int averageG_1;
kishibekairohan 2:c015739085d3 127 int averageB_1;
kishibekairohan 2:c015739085d3 128 int averageR_2;
kishibekairohan 2:c015739085d3 129 int averageG_2;
kishibekairohan 2:c015739085d3 130 int averageB_2;
kishibekairohan 2:c015739085d3 131 int averageR_3;
kishibekairohan 2:c015739085d3 132 int averageG_3;
kishibekairohan 2:c015739085d3 133 int averageB_3;
kishibekairohan 2:c015739085d3 134
kishibekairohan 2:c015739085d3 135 void ColorDetection();
kishibekairohan 2:c015739085d3 136
t_yamamoto 0:669ef71cba68 137 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 138
t_yamamoto 0:669ef71cba68 139 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 140 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 141 static void Process0(void);
t_yamamoto 0:669ef71cba68 142 #endif
t_yamamoto 0:669ef71cba68 143 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 144 static void Process1(void);
t_yamamoto 0:669ef71cba68 145 #endif
t_yamamoto 0:669ef71cba68 146 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 147 static void Process2(void);
t_yamamoto 0:669ef71cba68 148 #endif
t_yamamoto 0:669ef71cba68 149 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 150 static void Process3(void);
t_yamamoto 0:669ef71cba68 151 #endif
t_yamamoto 0:669ef71cba68 152 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 153 static void Process4(void);
t_yamamoto 0:669ef71cba68 154 #endif
t_yamamoto 0:669ef71cba68 155 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 156 static void Process5(void);
t_yamamoto 0:669ef71cba68 157 #endif
t_yamamoto 0:669ef71cba68 158 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 159 static void Process6(void);
t_yamamoto 0:669ef71cba68 160 #endif
t_yamamoto 0:669ef71cba68 161 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 162 static void Process7(void);
t_yamamoto 0:669ef71cba68 163 #endif
t_yamamoto 0:669ef71cba68 164 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 165 static void Process8(void);
t_yamamoto 0:669ef71cba68 166 #endif
t_yamamoto 0:669ef71cba68 167 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 168 static void Process9(void);
t_yamamoto 0:669ef71cba68 169 #endif
t_yamamoto 0:669ef71cba68 170 #endif
t_yamamoto 0:669ef71cba68 171
t_yamamoto 0:669ef71cba68 172 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 173 {
t_yamamoto 0:669ef71cba68 174 #pragma region USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 175
t_yamamoto 0:669ef71cba68 176 /*Replace here with the initialization code of your variables.*/
t_yamamoto 0:669ef71cba68 177
t_yamamoto 0:669ef71cba68 178 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 179
t_yamamoto 0:669ef71cba68 180 lock = true;
t_yamamoto 0:669ef71cba68 181 processChangeComp = true;
t_yamamoto 0:669ef71cba68 182 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 183
t_yamamoto 0:669ef71cba68 184 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 185 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 186 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 187 #endif
t_yamamoto 0:669ef71cba68 188 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 189 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 190 #endif
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 192 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 195 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 198 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 201 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 204 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 207 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 210 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 213 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #endif
t_yamamoto 0:669ef71cba68 216 }
t_yamamoto 0:669ef71cba68 217
t_yamamoto 0:669ef71cba68 218 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 219 {
t_yamamoto 0:669ef71cba68 220 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 221 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 222
t_yamamoto 0:669ef71cba68 223 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 224 {
t_yamamoto 0:669ef71cba68 225 current++;
t_yamamoto 0:669ef71cba68 226 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 227 processChangeComp = false;
t_yamamoto 0:669ef71cba68 228 }
t_yamamoto 0:669ef71cba68 229 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 230 {
t_yamamoto 0:669ef71cba68 231 current--;
t_yamamoto 0:669ef71cba68 232 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 233 processChangeComp = false;
t_yamamoto 0:669ef71cba68 234 }
t_yamamoto 0:669ef71cba68 235 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 236 #endif
t_yamamoto 0:669ef71cba68 237
t_yamamoto 0:669ef71cba68 238 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 239 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 240 #endif
t_yamamoto 0:669ef71cba68 241
t_yamamoto 0:669ef71cba68 242 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 243 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 244 #endif
t_yamamoto 0:669ef71cba68 245
t_yamamoto 0:669ef71cba68 246 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 247 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 248 #endif
t_yamamoto 0:669ef71cba68 249
t_yamamoto 0:669ef71cba68 250 }
t_yamamoto 0:669ef71cba68 251
kishibekairohan 2:c015739085d3 252
kishibekairohan 2:c015739085d3 253
t_yamamoto 0:669ef71cba68 254 void SystemProcess()
t_yamamoto 0:669ef71cba68 255 {
t_yamamoto 0:669ef71cba68 256 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 257
t_yamamoto 0:669ef71cba68 258 while(1)
kishibekairohan 2:c015739085d3 259 {
kishibekairohan 2:c015739085d3 260 float x = 0, y= 0, z = 0;
kishibekairohan 2:c015739085d3 261
kishibekairohan 2:c015739085d3 262 pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
kishibekairohan 2:c015739085d3 263
kishibekairohan 2:c015739085d3 264 x = acc[0]*1000;
kishibekairohan 2:c015739085d3 265 y = acc[1]*1000;
kishibekairohan 2:c015739085d3 266 z = acc[2]*1000;
kishibekairohan 2:c015739085d3 267
kishibekairohan 2:c015739085d3 268 pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
kishibekairohan 2:c015739085d3 269
kishibekairohan 2:c015739085d3 270 float rotateX = (x - 306)/2.22 - 90;
kishibekairohan 2:c015739085d3 271 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 2:c015739085d3 272 pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
kishibekairohan 2:c015739085d3 273 wait_ms(50);
kishibekairohan 2:c015739085d3 274
t_yamamoto 0:669ef71cba68 275 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 276 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 277 #endif
t_yamamoto 0:669ef71cba68 278
t_yamamoto 0:669ef71cba68 279 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 280 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 281 {
t_yamamoto 0:669ef71cba68 282 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 283 AllActuatorReset();
t_yamamoto 0:669ef71cba68 284 lock = true;
t_yamamoto 0:669ef71cba68 285 }
t_yamamoto 0:669ef71cba68 286 else
t_yamamoto 0:669ef71cba68 287 #endif
t_yamamoto 0:669ef71cba68 288 {
t_yamamoto 0:669ef71cba68 289
t_yamamoto 0:669ef71cba68 290 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 291 if(!lock)
t_yamamoto 0:669ef71cba68 292 {
t_yamamoto 0:669ef71cba68 293 Process[current]();
t_yamamoto 0:669ef71cba68 294 }
t_yamamoto 0:669ef71cba68 295 else
t_yamamoto 0:669ef71cba68 296 #endif
t_yamamoto 0:669ef71cba68 297 {
t_yamamoto 0:669ef71cba68 298 //ロック時の処理
t_yamamoto 0:669ef71cba68 299 }
t_yamamoto 0:669ef71cba68 300 }
t_yamamoto 0:669ef71cba68 301
t_yamamoto 0:669ef71cba68 302 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 303 }
t_yamamoto 0:669ef71cba68 304 }
t_yamamoto 0:669ef71cba68 305
kishibekairohan 2:c015739085d3 306
kishibekairohan 2:c015739085d3 307
kishibekairohan 2:c015739085d3 308
t_yamamoto 0:669ef71cba68 309 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 310 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 311 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 312 static void Process0()
t_yamamoto 0:669ef71cba68 313 {
kishibekairohan 2:c015739085d3 314 ColorDetection();
t_yamamoto 0:669ef71cba68 315 }
t_yamamoto 0:669ef71cba68 316 #endif
t_yamamoto 0:669ef71cba68 317
t_yamamoto 0:669ef71cba68 318 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 319 static void Process1()
t_yamamoto 0:669ef71cba68 320 {
kishibekairohan 2:c015739085d3 321 motor[0].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]) * 0.8;
kishibekairohan 2:c015739085d3 322 motor[1].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]) * 0.8;
kishibekairohan 2:c015739085d3 323 motor[2].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]) * 0.8;
kishibekairohan 2:c015739085d3 324 motor[3].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]) * 0.8;
kishibekairohan 2:c015739085d3 325
kishibekairohan 2:c015739085d3 326 motor[0].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
kishibekairohan 2:c015739085d3 327 motor[1].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
kishibekairohan 2:c015739085d3 328 motor[2].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
kishibekairohan 2:c015739085d3 329 motor[3].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
kishibekairohan 2:c015739085d3 330
kishibekairohan 2:c015739085d3 331 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 2:c015739085d3 332 motor[0].pwm = motor[0].pwm * 1.3;
kishibekairohan 2:c015739085d3 333 motor[1].pwm = motor[1].pwm * 1.3;
kishibekairohan 2:c015739085d3 334
kishibekairohan 2:c015739085d3 335 }
t_yamamoto 0:669ef71cba68 336 }
t_yamamoto 0:669ef71cba68 337 #endif
t_yamamoto 0:669ef71cba68 338
kishibekairohan 2:c015739085d3 339 bool buttoncomp = false;
t_yamamoto 0:669ef71cba68 340 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 341 static void Process2()
t_yamamoto 0:669ef71cba68 342 {
kishibekairohan 2:c015739085d3 343 ColorDetection();
kishibekairohan 2:c015739085d3 344
kishibekairohan 2:c015739085d3 345 if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 2:c015739085d3 346 {
kishibekairohan 2:c015739085d3 347 invationA ^= 1;//start false,over true
kishibekairohan 2:c015739085d3 348 compA = true;//on true,noon false
kishibekairohan 2:c015739085d3 349 }
kishibekairohan 2:c015739085d3 350 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
kishibekairohan 2:c015739085d3 351
kishibekairohan 2:c015739085d3 352 if(controller->Button.A && buttoncomp = false)
kishibekairohan 2:c015739085d3 353 {
kishibekairohan 2:c015739085d3 354 motor[0].dir = dir;
kishibekairohan 2:c015739085d3 355 motor[0].pwm = startP;
kishibekairohan 2:c015739085d3 356 }
kishibekairohan 2:c015739085d3 357
kishibekairohan 2:c015739085d3 358 if(invationA)
kishibekairohan 2:c015739085d3 359 {
kishibekairohan 2:c015739085d3 360 motor[0].PWM = startP
kishibekairohan 2:c015739085d3 361
kishibekairohan 2:c015739085d3 362 }
kishibekairohan 2:c015739085d3 363
kishibekairohan 2:c015739085d3 364
kishibekairohan 2:c015739085d3 365
kishibekairohan 2:c015739085d3 366
t_yamamoto 0:669ef71cba68 367 }
t_yamamoto 0:669ef71cba68 368 #endif
t_yamamoto 0:669ef71cba68 369
t_yamamoto 0:669ef71cba68 370 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 371 static void Process3()
t_yamamoto 0:669ef71cba68 372 {
kishibekairohan 2:c015739085d3 373 if(controller->Button.R){
kishibekairohan 2:c015739085d3 374 motor[4].dir = FOR;
kishibekairohan 2:c015739085d3 375 motor[5].dir = BACK;
kishibekairohan 2:c015739085d3 376 motor[4].pwm = 150;
kishibekairohan 2:c015739085d3 377 motor[5].pwm = 150;
kishibekairohan 2:c015739085d3 378 }else if(controller->Button.L){
kishibekairohan 2:c015739085d3 379 motor[4].dir = BACK;
kishibekairohan 2:c015739085d3 380 motor[5].dir = FOR;
kishibekairohan 2:c015739085d3 381 motor[4].pwm = 150;
kishibekairohan 2:c015739085d3 382 motor[5].pwm = 150;
kishibekairohan 2:c015739085d3 383 }else{
kishibekairohan 2:c015739085d3 384 motor[4].dir = BRAKE;
kishibekairohan 2:c015739085d3 385 motor[5].dir = BRAKE;
kishibekairohan 2:c015739085d3 386 }
kishibekairohan 2:c015739085d3 387 if(LimitSw::IsPressed(0)){
kishibekairohan 2:c015739085d3 388 motor[4].dir = BRAKE;
kishibekairohan 2:c015739085d3 389 motor[5].dir = BRAKE;
kishibekairohan 2:c015739085d3 390 }else if(LimitSw::IsPressed(1)){
kishibekairohan 2:c015739085d3 391 motor[4].dir = BRAKE;
kishibekairohan 2:c015739085d3 392 motor[5].dir = BRAKE;
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>4
t_yamamoto 0:669ef71cba68 398 static void Process4()
t_yamamoto 0:669ef71cba68 399 {
kishibekairohan 2:c015739085d3 400 //ColorDetection();
kishibekairohan 2:c015739085d3 401
kishibekairohan 2:c015739085d3 402 for(int i=0;i<=10;i++)
kishibekairohan 2:c015739085d3 403 {
kishibekairohan 2:c015739085d3 404 ColorDetection();
kishibekairohan 2:c015739085d3 405
kishibekairohan 2:c015739085d3 406 averageR_0 += Color_A[0];
kishibekairohan 2:c015739085d3 407 averageG_0 += Color_A[1];
kishibekairohan 2:c015739085d3 408 averageB_0 += Color_A[2];
kishibekairohan 2:c015739085d3 409 averageR_1 += Color_B[0];
kishibekairohan 2:c015739085d3 410 averageG_1 += Color_B[1];
kishibekairohan 2:c015739085d3 411 averageB_1 += Color_B[2];
kishibekairohan 2:c015739085d3 412 averageR_2 += Color_C[0];
kishibekairohan 2:c015739085d3 413 averageG_2 += Color_C[1];
kishibekairohan 2:c015739085d3 414 averageB_2 += Color_C[2];
kishibekairohan 2:c015739085d3 415 averageR_3 += Color_D[0];
kishibekairohan 2:c015739085d3 416 averageG_3 += Color_D[1];
kishibekairohan 2:c015739085d3 417 averageB_3 += Color_D[2];
kishibekairohan 2:c015739085d3 418 }
kishibekairohan 2:c015739085d3 419 pc.printf("AR_0:%d, AG_0:%d ,AB_0:%d \r\n",averageR_0 / 10 ,averageG_0 / 10, averageB_0 / 10);
kishibekairohan 2:c015739085d3 420 pc.printf("AR_1:%d, AG_1:%d ,AB_1:%d \r\n",averageR_1 / 10 ,averageG_1 / 10, averageB_1 / 10);
kishibekairohan 2:c015739085d3 421 pc.printf("AR_2:%d, AG_2:%d ,AB_2:%d \r\n",averageR_2 / 10 ,averageG_2 / 10, averageB_2 / 10);
kishibekairohan 2:c015739085d3 422 pc.printf("AR_3:%d, AG_3:%d ,AB_3:%d \r\n",averageR_3 / 10 ,averageG_3 / 10, averageB_3 / 10);
kishibekairohan 2:c015739085d3 423
kishibekairohan 2:c015739085d3 424 averageR_0 = 0;
kishibekairohan 2:c015739085d3 425 averageG_0 = 0;
kishibekairohan 2:c015739085d3 426 averageB_0 = 0;
kishibekairohan 2:c015739085d3 427 averageR_1 = 0;
kishibekairohan 2:c015739085d3 428 averageG_1 = 0;
kishibekairohan 2:c015739085d3 429 averageB_1 = 0;
kishibekairohan 2:c015739085d3 430 averageR_2 = 0;
kishibekairohan 2:c015739085d3 431 averageG_2 = 0;
kishibekairohan 2:c015739085d3 432 averageB_2 = 0;
kishibekairohan 2:c015739085d3 433 averageR_3 = 0;
kishibekairohan 2:c015739085d3 434 averageG_3 = 0;
kishibekairohan 2:c015739085d3 435 averageB_3 = 0;
t_yamamoto 0:669ef71cba68 436 }
t_yamamoto 0:669ef71cba68 437 #endif
t_yamamoto 0:669ef71cba68 438
t_yamamoto 0:669ef71cba68 439 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 440 static void Process5()
t_yamamoto 0:669ef71cba68 441 {
kishibekairohan 2:c015739085d3 442 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 443 //int rotateX = (acc[0].read()-)/ -90;
kishibekairohan 2:c015739085d3 444 //int rotateY = (acc[1].read()-)/ -90;
kishibekairohan 2:c015739085d3 445 //pc.printf("X:%d ,Y:%d", rotateX, rotateY);
kishibekairohan 2:c015739085d3 446 wait_ms(50);
t_yamamoto 0:669ef71cba68 447 }
t_yamamoto 0:669ef71cba68 448 #endif
t_yamamoto 0:669ef71cba68 449
t_yamamoto 0:669ef71cba68 450 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 451 static void Process6()
t_yamamoto 0:669ef71cba68 452 {
t_yamamoto 0:669ef71cba68 453
t_yamamoto 0:669ef71cba68 454 }
t_yamamoto 0:669ef71cba68 455 #endif
t_yamamoto 0:669ef71cba68 456
t_yamamoto 0:669ef71cba68 457 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 458 static void Process7()
t_yamamoto 0:669ef71cba68 459 {
t_yamamoto 0:669ef71cba68 460
t_yamamoto 0:669ef71cba68 461 }
t_yamamoto 0:669ef71cba68 462 #endif
t_yamamoto 0:669ef71cba68 463
t_yamamoto 0:669ef71cba68 464 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 465 static void Process8()
t_yamamoto 0:669ef71cba68 466 {
t_yamamoto 0:669ef71cba68 467
t_yamamoto 0:669ef71cba68 468 }
t_yamamoto 0:669ef71cba68 469 #endif
t_yamamoto 0:669ef71cba68 470
t_yamamoto 0:669ef71cba68 471 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 472 static void Process9()
t_yamamoto 0:669ef71cba68 473 {
t_yamamoto 0:669ef71cba68 474
t_yamamoto 0:669ef71cba68 475 }
t_yamamoto 0:669ef71cba68 476 #endif
t_yamamoto 0:669ef71cba68 477 #endif
t_yamamoto 0:669ef71cba68 478 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 479
t_yamamoto 0:669ef71cba68 480 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 481 {
t_yamamoto 0:669ef71cba68 482
t_yamamoto 0:669ef71cba68 483 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 484 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 485 #endif
t_yamamoto 0:669ef71cba68 486
t_yamamoto 0:669ef71cba68 487 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 488 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 489 {
t_yamamoto 0:669ef71cba68 490 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 491 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 492 }
t_yamamoto 0:669ef71cba68 493 #endif
t_yamamoto 0:669ef71cba68 494 }
t_yamamoto 0:669ef71cba68 495
t_yamamoto 0:669ef71cba68 496 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 2:c015739085d3 497 void ColorDetection(){
kishibekairohan 2:c015739085d3 498 GATE = 0;
kishibekairohan 2:c015739085d3 499
kishibekairohan 2:c015739085d3 500 CK[0] = 0;
kishibekairohan 2:c015739085d3 501 CK[1] = 0;
kishibekairohan 2:c015739085d3 502 CK[2] = 0;
kishibekairohan 2:c015739085d3 503 CK[3] = 0;
kishibekairohan 2:c015739085d3 504
kishibekairohan 2:c015739085d3 505 RANGE = 1;
kishibekairohan 2:c015739085d3 506
kishibekairohan 2:c015739085d3 507 GATE = 1;
kishibekairohan 2:c015739085d3 508 wait_ms(intergration);
kishibekairohan 2:c015739085d3 509 GATE = 0;
kishibekairohan 2:c015739085d3 510 wait_us(4);
kishibekairohan 2:c015739085d3 511
kishibekairohan 2:c015739085d3 512 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 513 wait_us(3);
kishibekairohan 2:c015739085d3 514 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 515 wait_us(3);
kishibekairohan 2:c015739085d3 516 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 517
kishibekairohan 2:c015739085d3 518 //pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d",Color_A[0],Color_A[1],Color_A[2]);
kishibekairohan 2:c015739085d3 519 //pc.printf("\r\n");
kishibekairohan 2:c015739085d3 520
kishibekairohan 2:c015739085d3 521 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 522 wait_us(3);
kishibekairohan 2:c015739085d3 523 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 524 wait_us(3);
kishibekairohan 2:c015739085d3 525 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 526
kishibekairohan 2:c015739085d3 527 //pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d",Color_B[0],Color_B[1],Color_B[2]);
kishibekairohan 2:c015739085d3 528 //pc.printf("\r\n");
kishibekairohan 2:c015739085d3 529
kishibekairohan 2:c015739085d3 530 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 531 wait_us(3);
kishibekairohan 2:c015739085d3 532 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 533 wait_us(3);
kishibekairohan 2:c015739085d3 534 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 535
kishibekairohan 2:c015739085d3 536 /*pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d",Color_C[0],Color_C[1],Color_C[2]);
kishibekairohan 2:c015739085d3 537 pc.printf("\r\n");*/
kishibekairohan 2:c015739085d3 538
kishibekairohan 2:c015739085d3 539 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 540 wait_us(3);
kishibekairohan 2:c015739085d3 541 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 542 wait_us(3);
kishibekairohan 2:c015739085d3 543 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 544
kishibekairohan 2:c015739085d3 545 /*pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d",Color_D[0],Color_D[1],Color_D[2]);
kishibekairohan 2:c015739085d3 546 pc.printf("\r\n");*/
kishibekairohan 2:c015739085d3 547 }
t_yamamoto 0:669ef71cba68 548 #pragma endregion