Akihiro Nakabayashi / Mbed 2 deprecated MainBoard2018_Auto_Master_A_awasetekudsai

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

Fork of MainBoard2018_Auto_Master_A_neww by Taiki Maruyama

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

Committer:
7ka884
Date:
Mon Oct 08 14:25:57 2018 +0000
Revision:
13:ddd605700e8a
Parent:
10:ce0421ec431d 
tt

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