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@9:f93fc79a49ea, 2018-10-04 (annotated)

Committer:
kishibekairohan
Date:
Thu Oct 04 12:14:25 2018 +0000
Revision:
9:f93fc79a49ea
Parent:
8:6fb3723f7747
Child:
10:1295d39fec3a
eee;

Who changed what in which revision?

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