Miya Miyagawa / Mbed 2 deprecated AutoMaster_si-tu

lknds

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@13:b6e02d6261d7, 2018-10-08 (annotated)

Committer:
kishibekairohan
Date:
Mon Oct 08 15:51:15 2018 +0000
Revision:
13:b6e02d6261d7
Parent:
12:c09b3e08a316
Child:
14:93776ca449a4
Child:
15:dfcec98f5aa9
aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa

Who changed what in which revision?

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