Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@7:e88c5d47a3be, 2018-10-01 (annotated)

Committer:
kishibekairohan
Date:
Mon Oct 01 14:45:50 2018 +0000
Revision:
7:e88c5d47a3be
Parent:
6:10e22bc327ce
Child:
8:6fb3723f7747
hakattana!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
7ka884 4:ba9df71868df 3 #include "QEI.h"
t_yamamoto 0:669ef71cba68 4
7ka884 4:ba9df71868df 5 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 6 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/Switch/Switch.h"
7ka884 1:b1219d8ca117 10 #include "../../Input/ColorSensor/ColorSensor.h"
7ka884 1:b1219d8ca117 11 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
7ka884 1:b1219d8ca117 13 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto 0:669ef71cba68 14 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 15 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 16 #include "../Using.h"
t_yamamoto 0:669ef71cba68 17
kishibekairohan 2:c015739085d3 18
t_yamamoto 0:669ef71cba68 19 using namespace SWITCH;
kishibekairohan 2:c015739085d3 20 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 21 using namespace ACCELERATIONSENSOR;
7ka884 4:ba9df71868df 22 using namespace PID_SPACE;
7ka884 4:ba9df71868df 23 using namespace ROTARYENCODER;
t_yamamoto 0:669ef71cba68 24
t_yamamoto 0:669ef71cba68 25 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 27 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 static bool lock;
t_yamamoto 0:669ef71cba68 30 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 31 static int current;
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 36 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 37 #endif
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 40
t_yamamoto 0:669ef71cba68 41 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 42
kishibekairohan 2:c015739085d3 43 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 44
kishibekairohan 2:c015739085d3 45 unsigned long ColorIn(int index)
kishibekairohan 2:c015739085d3 46 {
kishibekairohan 2:c015739085d3 47 int result = 0;
kishibekairohan 2:c015739085d3 48 bool rtn = false;
kishibekairohan 2:c015739085d3 49 for(int i=0; i<12; i++)
kishibekairohan 2:c015739085d3 50 {
kishibekairohan 2:c015739085d3 51 CK[index] = 1;
kishibekairohan 2:c015739085d3 52 rtn = DOUT[index];
kishibekairohan 2:c015739085d3 53 CK[index] = 0;
kishibekairohan 2:c015739085d3 54 if(rtn)
kishibekairohan 2:c015739085d3 55 {
kishibekairohan 2:c015739085d3 56 result|=(1 << i);
kishibekairohan 2:c015739085d3 57 }
kishibekairohan 2:c015739085d3 58 }
kishibekairohan 2:c015739085d3 59 return result;
kishibekairohan 2:c015739085d3 60 }
kishibekairohan 2:c015739085d3 61
kishibekairohan 7:e88c5d47a3be 62 #define TIRE_FR 4 //足回り前右
kishibekairohan 7:e88c5d47a3be 63 #define TIRE_FL 5 //足回り前左
7ka884 4:ba9df71868df 64 #define TIRE_BR 2 //足回り後右
7ka884 4:ba9df71868df 65 #define TIRE_BL 3 //足回り後左
kishibekairohan 2:c015739085d3 66
kishibekairohan 7:e88c5d47a3be 67 #define Angle_R 0 //角度調節右
kishibekairohan 7:e88c5d47a3be 68 #define Angle_L 1 //角度調節左
kishibekairohan 7:e88c5d47a3be 69
kishibekairohan 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 7:e88c5d47a3be 74
kishibekairohan 7:e88c5d47a3be 75 //************メカナム********************
kishibekairohan 2:c015739085d3 76
kishibekairohan 2:c015739085d3 77 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 78 {
kishibekairohan 2:c015739085d3 79 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 80 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 81 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 82 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 83 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 84 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 85 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 86 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 87 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 88 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 89 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 90 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 91 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 92 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 93 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 94 };
kishibekairohan 2:c015739085d3 95
kishibekairohan 2:c015739085d3 96 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 97 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 98 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 99 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 100 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 101 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 102 else return BRAKE;
kishibekairohan 2:c015739085d3 103 }
kishibekairohan 2:c015739085d3 104 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 105 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 106 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 107 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 108 }
kishibekairohan 2:c015739085d3 109
kishibekairohan 7:e88c5d47a3be 110 //************メカナム********************
kishibekairohan 7:e88c5d47a3be 111
kishibekairohan 7:e88c5d47a3be 112 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 113
kishibekairohan 2:c015739085d3 114 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 115 int Color_B[3];
kishibekairohan 2:c015739085d3 116 int Color_C[3];
kishibekairohan 2:c015739085d3 117 int Color_D[3];
kishibekairohan 2:c015739085d3 118 int intergration = 50;
kishibekairohan 2:c015739085d3 119
kishibekairohan 7:e88c5d47a3be 120 void ColorDetection();
kishibekairohan 7:e88c5d47a3be 121
kishibekairohan 7:e88c5d47a3be 122 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 123
kishibekairohan 2:c015739085d3 124 //************ライントレース変数*******************
kishibekairohan 7:e88c5d47a3be 125 int Point[3] = {234, 466, 590};//赤，緑，青
kishibekairohan 2:c015739085d3 126
kishibekairohan 7:e88c5d47a3be 127 int startP = 35;
kishibekairohan 7:e88c5d47a3be 128 int downP = 5;
kishibekairohan 2:c015739085d3 129 //************ライントレース変数*******************
7ka884 4:ba9df71868df 130 //ROタコン
7ka884 4:ba9df71868df 131 QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
7ka884 4:ba9df71868df 132
7ka884 4:ba9df71868df 133 Ticker get_rpm;
7ka884 4:ba9df71868df 134 int rpm;
7ka884 4:ba9df71868df 135 int palse;
7ka884 4:ba9df71868df 136 double goalpoint = 3000.0000;
7ka884 4:ba9df71868df 137
7ka884 4:ba9df71868df 138 PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
7ka884 4:ba9df71868df 139
kishibekairohan 7:e88c5d47a3be 140
kishibekairohan 7:e88c5d47a3be 141
kishibekairohan 2:c015739085d3 142
kishibekairohan 7:e88c5d47a3be 143 //************ジャイロ*******************
kishibekairohan 7:e88c5d47a3be 144 float Angle;
kishibekairohan 7:e88c5d47a3be 145 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan 7:e88c5d47a3be 146 bool Angle_flagX = false;
kishibekairohan 7:e88c5d47a3be 147 bool Angle_flagY = false;
kishibekairohan 7:e88c5d47a3be 148 bool Angle_flagI = false;
kishibekairohan 7:e88c5d47a3be 149 float rotateY;
kishibekairohan 7:e88c5d47a3be 150 int AngletargetX = 50;
kishibekairohan 7:e88c5d47a3be 151 int AngletargetY = -50;
kishibekairohan 7:e88c5d47a3be 152 int Angle_I = -5;
kishibekairohan 7:e88c5d47a3be 153 //************ジャイロ*******************
kishibekairohan 2:c015739085d3 154
kishibekairohan 2:c015739085d3 155
t_yamamoto 0:669ef71cba68 156 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 157
t_yamamoto 0:669ef71cba68 158 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 159 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 160 static void Process0(void);
t_yamamoto 0:669ef71cba68 161 #endif
t_yamamoto 0:669ef71cba68 162 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 163 static void Process1(void);
t_yamamoto 0:669ef71cba68 164 #endif
t_yamamoto 0:669ef71cba68 165 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 166 static void Process2(void);
t_yamamoto 0:669ef71cba68 167 #endif
t_yamamoto 0:669ef71cba68 168 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 169 static void Process3(void);
t_yamamoto 0:669ef71cba68 170 #endif
t_yamamoto 0:669ef71cba68 171 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 172 static void Process4(void);
t_yamamoto 0:669ef71cba68 173 #endif
t_yamamoto 0:669ef71cba68 174 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 175 static void Process5(void);
t_yamamoto 0:669ef71cba68 176 #endif
t_yamamoto 0:669ef71cba68 177 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 178 static void Process6(void);
t_yamamoto 0:669ef71cba68 179 #endif
t_yamamoto 0:669ef71cba68 180 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 181 static void Process7(void);
t_yamamoto 0:669ef71cba68 182 #endif
t_yamamoto 0:669ef71cba68 183 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 184 static void Process8(void);
t_yamamoto 0:669ef71cba68 185 #endif
t_yamamoto 0:669ef71cba68 186 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 187 static void Process9(void);
t_yamamoto 0:669ef71cba68 188 #endif
t_yamamoto 0:669ef71cba68 189 #endif
t_yamamoto 0:669ef71cba68 190
t_yamamoto 0:669ef71cba68 191 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 192 {
t_yamamoto 0:669ef71cba68 193 #pragma region USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 194
t_yamamoto 0:669ef71cba68 195 /*Replace here with the initialization code of your variables.*/
t_yamamoto 0:669ef71cba68 196
t_yamamoto 0:669ef71cba68 197 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 198
t_yamamoto 0:669ef71cba68 199 lock = true;
t_yamamoto 0:669ef71cba68 200 processChangeComp = true;
t_yamamoto 0:669ef71cba68 201 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 202
t_yamamoto 0:669ef71cba68 203 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 204 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 205 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 206 #endif
t_yamamoto 0:669ef71cba68 207 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 208 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 209 #endif
t_yamamoto 0:669ef71cba68 210 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 211 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 212 #endif
t_yamamoto 0:669ef71cba68 213 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 214 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 215 #endif
t_yamamoto 0:669ef71cba68 216 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 217 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 218 #endif
t_yamamoto 0:669ef71cba68 219 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 220 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 221 #endif
t_yamamoto 0:669ef71cba68 222 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 223 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 224 #endif
t_yamamoto 0:669ef71cba68 225 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 226 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 227 #endif
t_yamamoto 0:669ef71cba68 228 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 229 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 230 #endif
t_yamamoto 0:669ef71cba68 231 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 232 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 233 #endif
t_yamamoto 0:669ef71cba68 234 #endif
t_yamamoto 0:669ef71cba68 235 }
t_yamamoto 0:669ef71cba68 236
t_yamamoto 0:669ef71cba68 237 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 238 {
t_yamamoto 0:669ef71cba68 239 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 240 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 241
t_yamamoto 0:669ef71cba68 242 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 243 {
t_yamamoto 0:669ef71cba68 244 current++;
t_yamamoto 0:669ef71cba68 245 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 246 processChangeComp = false;
t_yamamoto 0:669ef71cba68 247 }
t_yamamoto 0:669ef71cba68 248 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 249 {
t_yamamoto 0:669ef71cba68 250 current--;
t_yamamoto 0:669ef71cba68 251 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 252 processChangeComp = false;
t_yamamoto 0:669ef71cba68 253 }
t_yamamoto 0:669ef71cba68 254 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 255 #endif
t_yamamoto 0:669ef71cba68 256
t_yamamoto 0:669ef71cba68 257 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 258 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 259 #endif
t_yamamoto 0:669ef71cba68 260
t_yamamoto 0:669ef71cba68 261 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 262 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 263 #endif
t_yamamoto 0:669ef71cba68 264
t_yamamoto 0:669ef71cba68 265 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 266 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 267 #endif
t_yamamoto 0:669ef71cba68 268
t_yamamoto 0:669ef71cba68 269 }
t_yamamoto 0:669ef71cba68 270
kishibekairohan 2:c015739085d3 271
kishibekairohan 2:c015739085d3 272
t_yamamoto 0:669ef71cba68 273 void SystemProcess()
t_yamamoto 0:669ef71cba68 274 {
t_yamamoto 0:669ef71cba68 275 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 276
t_yamamoto 0:669ef71cba68 277 while(1)
7ka884 4:ba9df71868df 278 {
7ka884 4:ba9df71868df 279 /*wait(0.1);
7ka884 4:ba9df71868df 280 //wheel.getPulses()...どちらの方向にどれだけ回ったか
7ka884 4:ba9df71868df 281 pc.printf("Pulses:%07d \r\n",wheel.getPulses());
7ka884 4:ba9df71868df 282 //軸が何回転したか
7ka884 4:ba9df71868df 283 pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
7ka884 4:ba9df71868df 284 */
7ka884 4:ba9df71868df 285
7ka884 4:ba9df71868df 286 /* float x = 0, y= 0, z = 0;
kishibekairohan 2:c015739085d3 287
kishibekairohan 2:c015739085d3 288 x = acc[0]*1000;
kishibekairohan 2:c015739085d3 289 y = acc[1]*1000;
kishibekairohan 2:c015739085d3 290 z = acc[2]*1000;
kishibekairohan 2:c015739085d3 291
kishibekairohan 2:c015739085d3 292 pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
kishibekairohan 2:c015739085d3 293
kishibekairohan 2:c015739085d3 294 float rotateX = (x - 306)/2.22 - 90;
kishibekairohan 2:c015739085d3 295 float rotateY = (y - 305)/2.21 - 90;
7ka884 4:ba9df71868df 296 float rotateZ = (z - 300)/1.18 - 90;
7ka884 4:ba9df71868df 297 pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
kishibekairohan 2:c015739085d3 298 wait_ms(50);
7ka884 4:ba9df71868df 299 */
7ka884 4:ba9df71868df 300 /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
7ka884 4:ba9df71868df 301 else LED_DEBUG0 = LED_OFF;*/
kishibekairohan 2:c015739085d3 302
t_yamamoto 0:669ef71cba68 303 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 304 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 305 #endif
t_yamamoto 0:669ef71cba68 306
t_yamamoto 0:669ef71cba68 307 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 308 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 309 {
t_yamamoto 0:669ef71cba68 310 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 311 AllActuatorReset();
t_yamamoto 0:669ef71cba68 312 lock = true;
t_yamamoto 0:669ef71cba68 313 }
t_yamamoto 0:669ef71cba68 314 else
t_yamamoto 0:669ef71cba68 315 #endif
t_yamamoto 0:669ef71cba68 316 {
t_yamamoto 0:669ef71cba68 317
t_yamamoto 0:669ef71cba68 318 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 319 if(!lock)
t_yamamoto 0:669ef71cba68 320 {
t_yamamoto 0:669ef71cba68 321 Process[current]();
t_yamamoto 0:669ef71cba68 322 }
t_yamamoto 0:669ef71cba68 323 else
t_yamamoto 0:669ef71cba68 324 #endif
t_yamamoto 0:669ef71cba68 325 {
t_yamamoto 0:669ef71cba68 326 //ロック時の処理
t_yamamoto 0:669ef71cba68 327 }
t_yamamoto 0:669ef71cba68 328 }
t_yamamoto 0:669ef71cba68 329
t_yamamoto 0:669ef71cba68 330 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 331 }
t_yamamoto 0:669ef71cba68 332 }
t_yamamoto 0:669ef71cba68 333
kishibekairohan 2:c015739085d3 334
kishibekairohan 2:c015739085d3 335
kishibekairohan 2:c015739085d3 336
t_yamamoto 0:669ef71cba68 337 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 338 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 339 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 340 static void Process0()
t_yamamoto 0:669ef71cba68 341 {
kishibekairohan 7:e88c5d47a3be 342 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 343 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 344 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 345 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 346 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 347 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 348 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 349 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 350 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 351 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 352 }
kishibekairohan 7:e88c5d47a3be 353 for(int i = 0;i<20;i++){
kishibekairohan 7:e88c5d47a3be 354 float y = 0;
kishibekairohan 7:e88c5d47a3be 355 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 356 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 357 Angle += rotateY;
kishibekairohan 7:e88c5d47a3be 358 }
kishibekairohan 7:e88c5d47a3be 359 Angle = Angle /20;
kishibekairohan 7:e88c5d47a3be 360 int gyropwm = gyro.SetPV(Angle,Angle_I);
kishibekairohan 7:e88c5d47a3be 361
kishibekairohan 7:e88c5d47a3be 362 if(controller->Button.A){
kishibekairohan 7:e88c5d47a3be 363 Angle_flagI = true;
kishibekairohan 7:e88c5d47a3be 364 }
kishibekairohan 7:e88c5d47a3be 365 if (Angle_flagI){
kishibekairohan 7:e88c5d47a3be 366 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 7:e88c5d47a3be 367 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 7:e88c5d47a3be 368 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 369 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 370 if(Angle_I - 2 < Angle && Angle < Angle_I + 2){
kishibekairohan 7:e88c5d47a3be 371 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 372 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 373 Angle_flagI = false;
kishibekairohan 7:e88c5d47a3be 374 }
kishibekairohan 7:e88c5d47a3be 375 }
t_yamamoto 0:669ef71cba68 376 }
t_yamamoto 0:669ef71cba68 377 #endif
t_yamamoto 0:669ef71cba68 378
t_yamamoto 0:669ef71cba68 379 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 380 static void Process1()
t_yamamoto 0:669ef71cba68 381 {
7ka884 4:ba9df71868df 382
7ka884 4:ba9df71868df 383
7ka884 4:ba9df71868df 384 motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 385 motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 386 motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 387 motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 2:c015739085d3 388
kishibekairohan 7:e88c5d47a3be 389 motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 390 motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 391 motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 7:e88c5d47a3be 392 motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 2:c015739085d3 393
kishibekairohan 2:c015739085d3 394 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 7:e88c5d47a3be 395 motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
kishibekairohan 7:e88c5d47a3be 396 motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
7ka884 4:ba9df71868df 397 }
7ka884 4:ba9df71868df 398
kishibekairohan 2:c015739085d3 399
7ka884 4:ba9df71868df 400 //wheel.getPulses()...どちらの方向にどれだけ回ったか
7ka884 4:ba9df71868df 401 pc.printf("Pulses:%07d \r\n",wheel.getPulses());
7ka884 4:ba9df71868df 402 //軸が何回転したか
7ka884 4:ba9df71868df 403 pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
t_yamamoto 0:669ef71cba68 404 }
t_yamamoto 0:669ef71cba68 405 #endif
t_yamamoto 0:669ef71cba68 406
t_yamamoto 0:669ef71cba68 407 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 408 static void Process2()
t_yamamoto 0:669ef71cba68 409 {
7ka884 4:ba9df71868df 410 static bool color_flag = false;
7ka884 4:ba9df71868df 411
7ka884 4:ba9df71868df 412 static bool traceon = false;//fase1
7ka884 4:ba9df71868df 413 static bool yokofla = false;//fase2
7ka884 4:ba9df71868df 414 static bool boxslip = false;//fase3
kishibekairohan 2:c015739085d3 415
7ka884 4:ba9df71868df 416 static bool compA = false;
7ka884 4:ba9df71868df 417 static bool compB = false;
7ka884 4:ba9df71868df 418 static bool compC = false;
7ka884 4:ba9df71868df 419 static bool compD = false;
7ka884 4:ba9df71868df 420
7ka884 4:ba9df71868df 421 static bool invationA = false;
7ka884 4:ba9df71868df 422 static bool invationB = false;
7ka884 4:ba9df71868df 423 static bool invationC = false;
7ka884 4:ba9df71868df 424 static bool invationD = false;
7ka884 4:ba9df71868df 425
7ka884 4:ba9df71868df 426 ColorDetection();
7ka884 4:ba9df71868df 427 //
kishibekairohan 2:c015739085d3 428 if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 2:c015739085d3 429 {
kishibekairohan 2:c015739085d3 430 invationA ^= 1;//start false,over true
kishibekairohan 2:c015739085d3 431 compA = true;//on true,noon false
kishibekairohan 2:c015739085d3 432 }
kishibekairohan 2:c015739085d3 433 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
kishibekairohan 2:c015739085d3 434
7ka884 4:ba9df71868df 435 if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
7ka884 4:ba9df71868df 436 {
7ka884 4:ba9df71868df 437 invationB ^= 1;//start false,over true
7ka884 4:ba9df71868df 438 compB = true;//on true,noon false
7ka884 4:ba9df71868df 439 }
7ka884 4:ba9df71868df 440 else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
7ka884 4:ba9df71868df 441
7ka884 4:ba9df71868df 442 if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
7ka884 4:ba9df71868df 443 {
7ka884 4:ba9df71868df 444 invationC ^= 1;//start false,over true
7ka884 4:ba9df71868df 445 compC = true;//on true,noon false
7ka884 4:ba9df71868df 446 }
7ka884 4:ba9df71868df 447 else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
7ka884 4:ba9df71868df 448
7ka884 4:ba9df71868df 449 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan 2:c015739085d3 450 {
7ka884 4:ba9df71868df 451 invationD ^= 1;//start false,over true
7ka884 4:ba9df71868df 452 compD = true;//on true,noon false
7ka884 4:ba9df71868df 453 }
7ka884 4:ba9df71868df 454 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
7ka884 4:ba9df71868df 455
7ka884 4:ba9df71868df 456
7ka884 4:ba9df71868df 457 //
7ka884 4:ba9df71868df 458
7ka884 4:ba9df71868df 459 if(controller->Button.B && !color_flag)
7ka884 4:ba9df71868df 460 {
7ka884 4:ba9df71868df 461 traceon ^= 1;
7ka884 4:ba9df71868df 462 color_flag = true;
kishibekairohan 2:c015739085d3 463 }
7ka884 4:ba9df71868df 464 else if(!controller->Button.B)color_flag = false;
kishibekairohan 2:c015739085d3 465
7ka884 4:ba9df71868df 466 if(traceon && !yokofla && !boxslip)
kishibekairohan 2:c015739085d3 467 {
7ka884 4:ba9df71868df 468 if(!invationA && !compA && !invationB && !compB)
7ka884 4:ba9df71868df 469 {
7ka884 4:ba9df71868df 470 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 471 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 472 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 473 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 474
7ka884 4:ba9df71868df 475 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 476 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 477 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 478 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 479 }
7ka884 4:ba9df71868df 480 else if(invationA && compA && !invationB && !compB)
7ka884 4:ba9df71868df 481 {
7ka884 4:ba9df71868df 482 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 483 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 484 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 485 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 486
7ka884 4:ba9df71868df 487 motor[TIRE_FR].pwm = startP - downP;
7ka884 4:ba9df71868df 488 motor[TIRE_FL].pwm = startP - downP;
7ka884 4:ba9df71868df 489 motor[TIRE_BR].pwm = startP - downP;
7ka884 4:ba9df71868df 490 motor[TIRE_BL].pwm = startP - downP;
7ka884 4:ba9df71868df 491 }
7ka884 4:ba9df71868df 492 else if(invationA && !compA && !invationB && !compB)
7ka884 4:ba9df71868df 493 {
7ka884 4:ba9df71868df 494 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 495 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 496 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 497 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 498
7ka884 4:ba9df71868df 499 wait(2);
7ka884 4:ba9df71868df 500
7ka884 4:ba9df71868df 501 yokofla = true;
7ka884 4:ba9df71868df 502 traceon = false;
7ka884 4:ba9df71868df 503 }
7ka884 4:ba9df71868df 504 else{
7ka884 4:ba9df71868df 505 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 506 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 507 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 508 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 509 }
7ka884 4:ba9df71868df 510 }
7ka884 4:ba9df71868df 511
7ka884 4:ba9df71868df 512 if(!traceon && yokofla && !boxslip)
7ka884 4:ba9df71868df 513 {
kishibekairohan 7:e88c5d47a3be 514 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
7ka884 4:ba9df71868df 515 {
7ka884 4:ba9df71868df 516 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 517 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 518 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 519 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 520
7ka884 4:ba9df71868df 521 wait(2);
7ka884 4:ba9df71868df 522
7ka884 4:ba9df71868df 523 boxslip = true;
7ka884 4:ba9df71868df 524 yokofla = false;
7ka884 4:ba9df71868df 525 }
7ka884 4:ba9df71868df 526 else if(invationA && !compA && invationB)
7ka884 4:ba9df71868df 527 {
7ka884 4:ba9df71868df 528 motor[TIRE_FR].dir = BACK;
7ka884 4:ba9df71868df 529 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 530 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 531 motor[TIRE_BL].dir = FOR;
7ka884 4:ba9df71868df 532
7ka884 4:ba9df71868df 533 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 534 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 535 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 536 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 537 }
7ka884 4:ba9df71868df 538 else if(!invationA && !compB && !invationB)
7ka884 4:ba9df71868df 539 {
7ka884 4:ba9df71868df 540 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 541 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 542 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 543 motor[TIRE_BL].dir = BACK;
kishibekairohan 2:c015739085d3 544
7ka884 4:ba9df71868df 545 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 546 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 547 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 548 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 549 }
7ka884 4:ba9df71868df 550 else if(invationA && compA && !invationB && !compB)
7ka884 4:ba9df71868df 551 {
7ka884 4:ba9df71868df 552 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 553 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 554 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 555 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 556
7ka884 4:ba9df71868df 557 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 558 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 559 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 560 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 561 }
7ka884 4:ba9df71868df 562 else if(compB && invationB)
7ka884 4:ba9df71868df 563 {
7ka884 4:ba9df71868df 564 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 565 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 566 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 567 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 568
7ka884 4:ba9df71868df 569 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 570 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 571 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 572 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 573 }
7ka884 4:ba9df71868df 574 else
7ka884 4:ba9df71868df 575 {
7ka884 4:ba9df71868df 576 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 577 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 578 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 579 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 580
7ka884 4:ba9df71868df 581 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 582 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 583 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 584 motor[TIRE_BL].pwm = startP;
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 = FOR;
7ka884 4:ba9df71868df 593 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 594 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 595 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 596
7ka884 4:ba9df71868df 597 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 598 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 599 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 600 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 601 }
kishibekairohan 7:e88c5d47a3be 602 else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
7ka884 4:ba9df71868df 603 {
7ka884 4:ba9df71868df 604 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 605 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 606 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 607 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 608 }
7ka884 4:ba9df71868df 609 }
7ka884 4:ba9df71868df 610 /*////
7ka884 4:ba9df71868df 611 motor[0].dir = BACK;
7ka884 4:ba9df71868df 612 motor[1].dir = BACK;
7ka884 4:ba9df71868df 613 motor[2].dir = FOR;
7ka884 4:ba9df71868df 614 motor[3].dir = FOR;
7ka884 4:ba9df71868df 615
7ka884 4:ba9df71868df 616 motor[0].pwm = startP;
7ka884 4:ba9df71868df 617 motor[1].pwm = startP;
7ka884 4:ba9df71868df 618 motor[2].pwm = startP;
7ka884 4:ba9df71868df 619 motor[3].pwm = startP;
7ka884 4:ba9df71868df 620 else if()
7ka884 4:ba9df71868df 621 {
7ka884 4:ba9df71868df 622 motor[0].dir = BRAKE;
7ka884 4:ba9df71868df 623 motor[1].dir = BRAKE;
7ka884 4:ba9df71868df 624 motor[2].dir = BRAKE;
7ka884 4:ba9df71868df 625 motor[3].dir = BRAKE;
7ka884 4:ba9df71868df 626
7ka884 4:ba9df71868df 627 motor[0].pwm = 255;
7ka884 4:ba9df71868df 628 motor[1].pwm = 255;
7ka884 4:ba9df71868df 629 motor[2].pwm = 255;
7ka884 4:ba9df71868df 630 motor[3].pwm = 255;
7ka884 4:ba9df71868df 631 }*///////
t_yamamoto 0:669ef71cba68 632 }
t_yamamoto 0:669ef71cba68 633 #endif
t_yamamoto 0:669ef71cba68 634
t_yamamoto 0:669ef71cba68 635 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 636 static void Process3()
t_yamamoto 0:669ef71cba68 637 {
kishibekairohan 2:c015739085d3 638 if(controller->Button.R){
kishibekairohan 7:e88c5d47a3be 639 motor[Angle_R].dir = FOR;
kishibekairohan 7:e88c5d47a3be 640 motor[Angle_L].dir = BACK;
kishibekairohan 7:e88c5d47a3be 641 motor[Angle_R].pwm = 150;
kishibekairohan 7:e88c5d47a3be 642 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 643 }else if(controller->Button.L){
kishibekairohan 7:e88c5d47a3be 644 motor[Angle_R].dir = BACK;
kishibekairohan 7:e88c5d47a3be 645 motor[Angle_L].dir = FOR;
kishibekairohan 7:e88c5d47a3be 646 motor[Angle_R].pwm = 150;
kishibekairohan 7:e88c5d47a3be 647 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 648 }else{
kishibekairohan 7:e88c5d47a3be 649 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 650 motor[Angle_L].dir = BRAKE;
7ka884 4:ba9df71868df 651 }
7ka884 4:ba9df71868df 652
kishibekairohan 7:e88c5d47a3be 653 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 654 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 655 motor[Angle_L].dir = BRAKE;
7ka884 4:ba9df71868df 656
kishibekairohan 7:e88c5d47a3be 657 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 658 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 659 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 660 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 661 motor[Angle_L].dir = BRAKE;
7ka884 4:ba9df71868df 662
kishibekairohan 7:e88c5d47a3be 663 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 664 motor[Angle_L].pwm = 255;
7ka884 4:ba9df71868df 665 }
t_yamamoto 0:669ef71cba68 666 }
t_yamamoto 0:669ef71cba68 667 #endif
t_yamamoto 0:669ef71cba68 668
t_yamamoto 0:669ef71cba68 669 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 670 static void Process4()
t_yamamoto 0:669ef71cba68 671 {
7ka884 4:ba9df71868df 672
kishibekairohan 7:e88c5d47a3be 673 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 674 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 675 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 676 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 677 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 678 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 679 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 680 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 681 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 682 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 683 }
kishibekairohan 7:e88c5d47a3be 684 for(int i = 0;i<20;i++){
kishibekairohan 7:e88c5d47a3be 685 float y = 0;
kishibekairohan 7:e88c5d47a3be 686 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 687 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 688 Angle += rotateY;
kishibekairohan 7:e88c5d47a3be 689 }
kishibekairohan 7:e88c5d47a3be 690 Angle = Angle /20;
kishibekairohan 7:e88c5d47a3be 691 int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan 7:e88c5d47a3be 692 int gyropwmY = gyro.SetPV(Angle,AngletargetY);
kishibekairohan 7:e88c5d47a3be 693
kishibekairohan 7:e88c5d47a3be 694 if(controller->Button.X){
kishibekairohan 7:e88c5d47a3be 695 Angle_flagX = true;
kishibekairohan 7:e88c5d47a3be 696 }
kishibekairohan 7:e88c5d47a3be 697 if(controller->Button.Y){
kishibekairohan 7:e88c5d47a3be 698 Angle_flagY = true;
kishibekairohan 7:e88c5d47a3be 699 }
kishibekairohan 7:e88c5d47a3be 700
kishibekairohan 7:e88c5d47a3be 701 if (Angle_flagX){
kishibekairohan 7:e88c5d47a3be 702 motor[Angle_R].dir = SetStatus(gyropwmX);
kishibekairohan 7:e88c5d47a3be 703 motor[Angle_L].dir = SetStatus(-gyropwmX);
kishibekairohan 7:e88c5d47a3be 704 motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan 7:e88c5d47a3be 705 motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan 7:e88c5d47a3be 706 if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan 7:e88c5d47a3be 707 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 708 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 709 Angle_flagX = false;
kishibekairohan 7:e88c5d47a3be 710 }
kishibekairohan 7:e88c5d47a3be 711 }
kishibekairohan 7:e88c5d47a3be 712
kishibekairohan 7:e88c5d47a3be 713 if (Angle_flagY){
kishibekairohan 7:e88c5d47a3be 714 motor[Angle_R].dir = SetStatus(gyropwmY);
kishibekairohan 7:e88c5d47a3be 715 motor[Angle_L].dir = SetStatus(-gyropwmY);
kishibekairohan 7:e88c5d47a3be 716 motor[Angle_R].pwm = SetPWM(gyropwmY);
kishibekairohan 7:e88c5d47a3be 717 motor[Angle_L].pwm = SetPWM(gyropwmY);
kishibekairohan 7:e88c5d47a3be 718 if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
kishibekairohan 7:e88c5d47a3be 719 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 720 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 721 Angle_flagY = false;
kishibekairohan 7:e88c5d47a3be 722 }
kishibekairohan 7:e88c5d47a3be 723 }
kishibekairohan 7:e88c5d47a3be 724 /*float y = 0;
kishibekairohan 7:e88c5d47a3be 725 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 726 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 727 int gyropwm = gyro.SetPV(rotateY , Angletarget);
kishibekairohan 7:e88c5d47a3be 728
kishibekairohan 7:e88c5d47a3be 729 if(controller->Button.X){
kishibekairohan 7:e88c5d47a3be 730 Angle_flag = true;
kishibekairohan 7:e88c5d47a3be 731 }
kishibekairohan 7:e88c5d47a3be 732 if (Angle_flag){
kishibekairohan 7:e88c5d47a3be 733 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 7:e88c5d47a3be 734 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 7:e88c5d47a3be 735 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 736 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 737 if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
kishibekairohan 7:e88c5d47a3be 738 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 739 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 740 Angle_flag = false;
kishibekairohan 7:e88c5d47a3be 741 }
kishibekairohan 7:e88c5d47a3be 742 }*/
kishibekairohan 7:e88c5d47a3be 743 else{
kishibekairohan 7:e88c5d47a3be 744 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 745 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 746 }
t_yamamoto 0:669ef71cba68 747 }
t_yamamoto 0:669ef71cba68 748 #endif
t_yamamoto 0:669ef71cba68 749
t_yamamoto 0:669ef71cba68 750 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 751 static void Process5()
t_yamamoto 0:669ef71cba68 752 {
kishibekairohan 7:e88c5d47a3be 753
t_yamamoto 0:669ef71cba68 754 }
t_yamamoto 0:669ef71cba68 755 #endif
t_yamamoto 0:669ef71cba68 756
t_yamamoto 0:669ef71cba68 757 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 758 static void Process6()
t_yamamoto 0:669ef71cba68 759 {
kishibekairohan 7:e88c5d47a3be 760
t_yamamoto 0:669ef71cba68 761 }
t_yamamoto 0:669ef71cba68 762 #endif
t_yamamoto 0:669ef71cba68 763
t_yamamoto 0:669ef71cba68 764 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 765 static void Process7()
t_yamamoto 0:669ef71cba68 766 {
7ka884 4:ba9df71868df 767
t_yamamoto 0:669ef71cba68 768 }
t_yamamoto 0:669ef71cba68 769 #endif
t_yamamoto 0:669ef71cba68 770
t_yamamoto 0:669ef71cba68 771 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 772 static void Process8()
t_yamamoto 0:669ef71cba68 773 {
t_yamamoto 0:669ef71cba68 774
t_yamamoto 0:669ef71cba68 775 }
t_yamamoto 0:669ef71cba68 776 #endif
t_yamamoto 0:669ef71cba68 777
t_yamamoto 0:669ef71cba68 778 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 779 static void Process9()
t_yamamoto 0:669ef71cba68 780 {
t_yamamoto 0:669ef71cba68 781
t_yamamoto 0:669ef71cba68 782 }
t_yamamoto 0:669ef71cba68 783 #endif
t_yamamoto 0:669ef71cba68 784 #endif
t_yamamoto 0:669ef71cba68 785 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 786
t_yamamoto 0:669ef71cba68 787 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 788 {
t_yamamoto 0:669ef71cba68 789
t_yamamoto 0:669ef71cba68 790 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 791 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 792 #endif
t_yamamoto 0:669ef71cba68 793
t_yamamoto 0:669ef71cba68 794 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 795 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 796 {
t_yamamoto 0:669ef71cba68 797 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 798 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 799 }
t_yamamoto 0:669ef71cba68 800 #endif
t_yamamoto 0:669ef71cba68 801 }
t_yamamoto 0:669ef71cba68 802
t_yamamoto 0:669ef71cba68 803 #pragma region USER-DEFINED-FUNCTIONS
7ka884 6:10e22bc327ce 804 void rotconpulex()
7ka884 6:10e22bc327ce 805 {
7ka884 6:10e22bc327ce 806
7ka884 6:10e22bc327ce 807 }
7ka884 6:10e22bc327ce 808
7ka884 6:10e22bc327ce 809
kishibekairohan 2:c015739085d3 810 void ColorDetection(){
kishibekairohan 2:c015739085d3 811 GATE = 0;
kishibekairohan 2:c015739085d3 812
kishibekairohan 2:c015739085d3 813 CK[0] = 0;
kishibekairohan 2:c015739085d3 814 CK[1] = 0;
kishibekairohan 2:c015739085d3 815 CK[2] = 0;
kishibekairohan 2:c015739085d3 816 CK[3] = 0;
kishibekairohan 2:c015739085d3 817
kishibekairohan 2:c015739085d3 818 RANGE = 1;
kishibekairohan 2:c015739085d3 819
kishibekairohan 2:c015739085d3 820 GATE = 1;
kishibekairohan 2:c015739085d3 821 wait_ms(intergration);
kishibekairohan 2:c015739085d3 822 GATE = 0;
kishibekairohan 2:c015739085d3 823 wait_us(4);
kishibekairohan 2:c015739085d3 824
kishibekairohan 2:c015739085d3 825 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 826 wait_us(3);
kishibekairohan 2:c015739085d3 827 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 828 wait_us(3);
kishibekairohan 2:c015739085d3 829 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 830
kishibekairohan 2:c015739085d3 831 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 832 wait_us(3);
kishibekairohan 2:c015739085d3 833 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 834 wait_us(3);
kishibekairohan 2:c015739085d3 835 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 836
kishibekairohan 2:c015739085d3 837 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 838 wait_us(3);
kishibekairohan 2:c015739085d3 839 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 840 wait_us(3);
kishibekairohan 2:c015739085d3 841 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 842
kishibekairohan 2:c015739085d3 843 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 844 wait_us(3);
kishibekairohan 2:c015739085d3 845 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 846 wait_us(3);
kishibekairohan 2:c015739085d3 847 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 848 }
t_yamamoto 0:669ef71cba68 849 #pragma endregion