大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_16

da

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@10:1295d39fec3a, 2018-10-05 (annotated)

Committer:
kishibekairohan
Date:
Fri Oct 05 12:17:21 2018 +0000
Revision:
10:1295d39fec3a
Parent:
9:f93fc79a49ea
Child:
11:028a150943b5
a

Who changed what in which revision?

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