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@8:6fb3723f7747, 2018-10-02 (annotated)

Committer:
kishibekairohan
Date:
Tue Oct 02 12:18:05 2018 +0000
Revision:
8:6fb3723f7747
Parent:
7:e88c5d47a3be
Child:
9:f93fc79a49ea
uwaaaaaaaaaaaaaaaaaaaa;

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