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@10:ce0421ec431d, 2018-10-06 (annotated)

Committer:
7ka884
Date:
Sat Oct 06 17:05:33 2018 +0000
Revision:
10:ce0421ec431d
Parent:
9:f93fc79a49ea
Child:
13:ddd605700e8a
awasetekudasai

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