Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_neww

aaaaaaaaa

Dependencies:   QEI mbed

Fork of MainBoard2018_Auto_Master_A_new by Akihiro Nakabayashi

System/Process/Process.cpp@12:c09b3e08a316, 2018-10-07 (annotated)

Committer:
kishibekairohan
Date:
Sun Oct 07 09:08:18 2018 +0000
Revision:
12:c09b3e08a316
Parent:
11:028a150943b5
Child:
13:b6e02d6261d7
aaaa;

Who changed what in which revision?

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