Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@22:7d93f79a3686, 2019-09-17 (annotated)

Committer:
M_souta
Date:
Tue Sep 17 04:40:17 2019 +0000
Revision:
22:7d93f79a3686
Parent:
21:e3b58d675c1c
Child:
23:c853372cf626
kkk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.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"
M_souta 21:e3b58d675c1c 7 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/Switch/Switch.h"
kishibekairohan 16:3f2c2d89372b 11 #include "../../Input/Encoder/Encoder.h"
M_souta 22:7d93f79a3686 12 #include "../../Input/Ultrasonic/Ultrasonic.h"
t_yamamoto 0:669ef71cba68 13 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 14 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 15 #include "../Using.h"
t_yamamoto 0:669ef71cba68 16
t_yamamoto 0:669ef71cba68 17 using namespace SWITCH;
7ka884 4:ba9df71868df 18 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 19 using namespace ENCODER;
M_souta 22:7d93f79a3686 20 using namespace ULTRASONIC;
M_souta 21:e3b58d675c1c 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
M_souta 22:7d93f79a3686 23
t_yamamoto 0:669ef71cba68 24 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 27
t_yamamoto 0:669ef71cba68 28 static bool lock;
t_yamamoto 0:669ef71cba68 29 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 30 static int current;
t_yamamoto 0:669ef71cba68 31
t_yamamoto 0:669ef71cba68 32 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 35 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 36 #endif
t_yamamoto 0:669ef71cba68 37
t_yamamoto 0:669ef71cba68 38 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 41
M_souta 22:7d93f79a3686 42 USS ultrasonic[] = {
M_souta 22:7d93f79a3686 43 USS(ECHO_0,TRIG_0,TEMP),
M_souta 22:7d93f79a3686 44 USS(ECHO_1,TRIG_1,TEMP),
M_souta 22:7d93f79a3686 45 };
kishibekairohan 2:c015739085d3 46
M_souta 22:7d93f79a3686 47 Serial pc(USBTX, USBRX);
kishibekairohan 7:e88c5d47a3be 48
kishibekairohan 16:3f2c2d89372b 49 //**************Buzzer****************
kishibekairohan 11:028a150943b5 50 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 51 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 52 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 53 bool EMGflag = false;
kishibekairohan 16:3f2c2d89372b 54 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 55 //**************Buzzer****************
kishibekairohan 11:028a150943b5 56
kishibekairohan 11:028a150943b5 57 //************TapeLed*****************
kishibekairohan 11:028a150943b5 58 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 59 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 60 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 61 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 62 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 63 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 64
M_souta 22:7d93f79a3686 65 //*************** lift ***************
M_souta 22:7d93f79a3686 66 #define LOWER 1
M_souta 22:7d93f79a3686 67 #define MIDDLRE 2
M_souta 22:7d93f79a3686 68 #define UPPER 3
M_souta 22:7d93f79a3686 69 uint8_t liftState = LOWER;
M_souta 22:7d93f79a3686 70 bool moving = false;
M_souta 22:7d93f79a3686 71 bool switchFlag_LB = false;
M_souta 22:7d93f79a3686 72 bool switchFlag_RB = false;
M_souta 22:7d93f79a3686 73
M_souta 22:7d93f79a3686 74 //*************** lift ***************
M_souta 22:7d93f79a3686 75
M_souta 22:7d93f79a3686 76 //*************tire*************
M_souta 22:7d93f79a3686 77 PID rotaconPID[] = {
M_souta 22:7d93f79a3686 78 PID(0.0001,-1,1,0.05,0,0), //LF
M_souta 22:7d93f79a3686 79 PID(0.0001,-1,1,0.05,0,0), //LB
M_souta 22:7d93f79a3686 80 PID(0.0001,-1,1,0.05,0,0), //RB
M_souta 22:7d93f79a3686 81 PID(0.0001,-1,1,0.05,0,0), //RF
M_souta 22:7d93f79a3686 82 };
M_souta 22:7d93f79a3686 83
M_souta 22:7d93f79a3686 84 #define FL 0
M_souta 22:7d93f79a3686 85 #define BL 1
M_souta 22:7d93f79a3686 86 #define BR 2
M_souta 22:7d93f79a3686 87 #define FR 3
M_souta 22:7d93f79a3686 88
M_souta 22:7d93f79a3686 89 #define PI 3.141592
M_souta 22:7d93f79a3686 90
M_souta 22:7d93f79a3686 91 const float tireR = 101.6; //タイヤの半径
M_souta 22:7d93f79a3686 92 const float ucR = 420.0; //中心からのタイヤの距離
M_souta 22:7d93f79a3686 93
M_souta 22:7d93f79a3686 94 typedef struct {
M_souta 22:7d93f79a3686 95 float Vx; //X方向の速度
M_souta 22:7d93f79a3686 96 float Vy; //Y方向の速度
M_souta 22:7d93f79a3686 97 float Va; //角速度
M_souta 22:7d93f79a3686 98 } Vvector;
M_souta 22:7d93f79a3686 99
M_souta 22:7d93f79a3686 100 Vvector move; //進む速度
M_souta 22:7d93f79a3686 101 Vvector correction_LT; //ライントレースの補正速度
M_souta 22:7d93f79a3686 102 Vvector synthetic; //合成速度
M_souta 22:7d93f79a3686 103
M_souta 22:7d93f79a3686 104 float sita = 0;
M_souta 22:7d93f79a3686 105
M_souta 22:7d93f79a3686 106 bool PIDflag = false;
M_souta 22:7d93f79a3686 107
M_souta 22:7d93f79a3686 108 int linePara[8];
M_souta 22:7d93f79a3686 109 int linePara_U;
M_souta 22:7d93f79a3686 110 int linePara_B;
M_souta 22:7d93f79a3686 111 int linePara_L;
M_souta 22:7d93f79a3686 112 int linePara_R;
M_souta 22:7d93f79a3686 113
M_souta 22:7d93f79a3686 114 #define FL 0
M_souta 22:7d93f79a3686 115 #define BL 1
M_souta 22:7d93f79a3686 116 #define BR 2
M_souta 22:7d93f79a3686 117 #define FR 3
M_souta 22:7d93f79a3686 118
M_souta 22:7d93f79a3686 119 float tireProcessRPM[4];
M_souta 22:7d93f79a3686 120 float tireTargetMaxRPM[4];
M_souta 22:7d93f79a3686 121 float tireTargetRPM[4];
M_souta 22:7d93f79a3686 122
M_souta 22:7d93f79a3686 123 float tirePWM[4];
M_souta 22:7d93f79a3686 124
M_souta 22:7d93f79a3686 125 float timePV[4];
M_souta 22:7d93f79a3686 126 float timeCV[4];
M_souta 22:7d93f79a3686 127 float pulsePV[4];
M_souta 22:7d93f79a3686 128 float pulseCV[4];
M_souta 22:7d93f79a3686 129
M_souta 22:7d93f79a3686 130 void tirePID();
M_souta 22:7d93f79a3686 131 int lineCast(char k);
M_souta 22:7d93f79a3686 132
M_souta 22:7d93f79a3686 133 Timer rotaconSampling;
M_souta 22:7d93f79a3686 134 Ticker rotaconPIDtimer;
M_souta 22:7d93f79a3686 135
M_souta 22:7d93f79a3686 136 bool countFlag;
M_souta 22:7d93f79a3686 137 //*************tire**************//
M_souta 22:7d93f79a3686 138
M_souta 22:7d93f79a3686 139 // ************* Line ************** //
M_souta 22:7d93f79a3686 140
M_souta 22:7d93f79a3686 141 float pw = 0;
M_souta 22:7d93f79a3686 142 int lineFase = 0;
M_souta 22:7d93f79a3686 143 bool lineCheck = false;
M_souta 22:7d93f79a3686 144 int linePWM;
M_souta 22:7d93f79a3686 145 int adj_F;
M_souta 22:7d93f79a3686 146 int adj_B;
M_souta 22:7d93f79a3686 147
M_souta 22:7d93f79a3686 148 int mode = 0;
M_souta 22:7d93f79a3686 149
M_souta 22:7d93f79a3686 150 int lineCount = 0;
M_souta 22:7d93f79a3686 151
M_souta 22:7d93f79a3686 152 // ************* Line ************** //
M_souta 22:7d93f79a3686 153
M_souta 21:e3b58d675c1c 154 const int omni[15][15] =
M_souta 20:eae8c84f318c 155 {
M_souta 21:e3b58d675c1c 156 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 157 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 158 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta 21:e3b58d675c1c 159 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta 21:e3b58d675c1c 160 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta 21:e3b58d675c1c 161 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta 21:e3b58d675c1c 162 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta 21:e3b58d675c1c 163 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta 21:e3b58d675c1c 164 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta 21:e3b58d675c1c 165 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta 21:e3b58d675c1c 166 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta 21:e3b58d675c1c 167 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta 21:e3b58d675c1c 168 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta 21:e3b58d675c1c 169 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta 21:e3b58d675c1c 170 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 171 };
M_souta 21:e3b58d675c1c 172
M_souta 21:e3b58d675c1c 173 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 174
M_souta 21:e3b58d675c1c 175 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 176 uint8_t SetStatus(int pwmVal) {
M_souta 21:e3b58d675c1c 177 if (pwmVal < 0) return BACK;
M_souta 21:e3b58d675c1c 178 else if (pwmVal > 0) return FOR;
M_souta 21:e3b58d675c1c 179 else if (pwmVal == 0) return BRAKE;
M_souta 21:e3b58d675c1c 180 else return BRAKE;
M_souta 20:eae8c84f318c 181 }
M_souta 21:e3b58d675c1c 182 uint8_t SetPWM(int);
M_souta 21:e3b58d675c1c 183 uint8_t SetPWM(int pwmVal) {
M_souta 21:e3b58d675c1c 184 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 21:e3b58d675c1c 185 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 186 }
M_souta 18:c694bae76e51 187
t_yamamoto 0:669ef71cba68 188 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 189
t_yamamoto 0:669ef71cba68 190 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 192 static void Process0(void);
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 195 static void Process1(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 198 static void Process2(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 201 static void Process3(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 204 static void Process4(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 207 static void Process5(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 210 static void Process6(void);
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 213 static void Process7(void);
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 216 static void Process8(void);
t_yamamoto 0:669ef71cba68 217 #endif
t_yamamoto 0:669ef71cba68 218 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 219 static void Process9(void);
t_yamamoto 0:669ef71cba68 220 #endif
t_yamamoto 0:669ef71cba68 221 #endif
t_yamamoto 0:669ef71cba68 222
t_yamamoto 0:669ef71cba68 223 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 224 {
t_yamamoto 0:669ef71cba68 225 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 16:3f2c2d89372b 226 /*Replace here with the initialization code of your variables.*/
M_souta 22:7d93f79a3686 227 rotaconPIDtimer.attach(tirePID,0.1);
M_souta 21:e3b58d675c1c 228
t_yamamoto 0:669ef71cba68 229 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 230
t_yamamoto 0:669ef71cba68 231 lock = true;
t_yamamoto 0:669ef71cba68 232 processChangeComp = true;
t_yamamoto 0:669ef71cba68 233 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 234
t_yamamoto 0:669ef71cba68 235 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 236 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 237 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 238 #endif
t_yamamoto 0:669ef71cba68 239 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 240 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 241 #endif
t_yamamoto 0:669ef71cba68 242 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 243 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 244 #endif
t_yamamoto 0:669ef71cba68 245 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 246 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 247 #endif
t_yamamoto 0:669ef71cba68 248 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 249 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 250 #endif
t_yamamoto 0:669ef71cba68 251 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 252 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 253 #endif
t_yamamoto 0:669ef71cba68 254 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 255 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 256 #endif
t_yamamoto 0:669ef71cba68 257 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 258 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 259 #endif
t_yamamoto 0:669ef71cba68 260 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 261 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 262 #endif
t_yamamoto 0:669ef71cba68 263 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 264 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 265 #endif
t_yamamoto 0:669ef71cba68 266 #endif
t_yamamoto 0:669ef71cba68 267 }
t_yamamoto 0:669ef71cba68 268
t_yamamoto 0:669ef71cba68 269 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 270 {
t_yamamoto 0:669ef71cba68 271 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 272 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 273
t_yamamoto 0:669ef71cba68 274 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 275 {
t_yamamoto 0:669ef71cba68 276 current++;
t_yamamoto 0:669ef71cba68 277 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 278 processChangeComp = false;
t_yamamoto 0:669ef71cba68 279 }
t_yamamoto 0:669ef71cba68 280 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 281 {
t_yamamoto 0:669ef71cba68 282 current--;
t_yamamoto 0:669ef71cba68 283 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 284 processChangeComp = false;
t_yamamoto 0:669ef71cba68 285 }
t_yamamoto 0:669ef71cba68 286 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 287 #endif
t_yamamoto 0:669ef71cba68 288
t_yamamoto 0:669ef71cba68 289 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 290 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 291 #endif
t_yamamoto 0:669ef71cba68 292
t_yamamoto 0:669ef71cba68 293 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 294 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 295 #endif
t_yamamoto 0:669ef71cba68 296
t_yamamoto 0:669ef71cba68 297 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 298 ACTUATORHUB::ActuatorHub::Update();
M_souta 21:e3b58d675c1c 299 //LINEHUB::LineHub::Update();
t_yamamoto 0:669ef71cba68 300 #endif
t_yamamoto 0:669ef71cba68 301
t_yamamoto 0:669ef71cba68 302 }
t_yamamoto 0:669ef71cba68 303
kishibekairohan 2:c015739085d3 304
kishibekairohan 2:c015739085d3 305
t_yamamoto 0:669ef71cba68 306 void SystemProcess()
t_yamamoto 0:669ef71cba68 307 {
t_yamamoto 0:669ef71cba68 308 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 309
t_yamamoto 0:669ef71cba68 310 while(1)
kishibekairohan 15:dfcec98f5aa9 311 {
M_souta 21:e3b58d675c1c 312 int g[8];
M_souta 21:e3b58d675c1c 313 for(int i = 0; i < 8; i++){
M_souta 22:7d93f79a3686 314 g[i] = lineCast(LineHub::GetPara(i));
M_souta 21:e3b58d675c1c 315 }
M_souta 22:7d93f79a3686 316
M_souta 22:7d93f79a3686 317 pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
M_souta 22:7d93f79a3686 318
M_souta 22:7d93f79a3686 319 //float a = ultrasonic[0].ReadDis();
M_souta 22:7d93f79a3686 320 //pc.printf("%f\n\r",a);
M_souta 22:7d93f79a3686 321
M_souta 22:7d93f79a3686 322 //int ppap = encoder[0].getPulses();
M_souta 22:7d93f79a3686 323 //pc.printf("%d\n\r",ppap);
M_souta 22:7d93f79a3686 324
M_souta 20:eae8c84f318c 325
kishibekairohan 17:50dc4b449e69 326 buzzer.period(1.0/800);
kishibekairohan 15:dfcec98f5aa9 327
t_yamamoto 0:669ef71cba68 328 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 329 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 330 #endif
t_yamamoto 0:669ef71cba68 331
t_yamamoto 0:669ef71cba68 332 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 333 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 334 {
t_yamamoto 0:669ef71cba68 335 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 336 AllActuatorReset();
t_yamamoto 0:669ef71cba68 337 lock = true;
t_yamamoto 0:669ef71cba68 338 }
t_yamamoto 0:669ef71cba68 339 else
t_yamamoto 0:669ef71cba68 340 #endif
t_yamamoto 0:669ef71cba68 341 {
t_yamamoto 0:669ef71cba68 342
t_yamamoto 0:669ef71cba68 343 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 344 if(!lock)
t_yamamoto 0:669ef71cba68 345 {
t_yamamoto 0:669ef71cba68 346 Process[current]();
t_yamamoto 0:669ef71cba68 347 }
t_yamamoto 0:669ef71cba68 348 else
t_yamamoto 0:669ef71cba68 349 #endif
t_yamamoto 0:669ef71cba68 350 {
t_yamamoto 0:669ef71cba68 351 //ロック時の処理
t_yamamoto 0:669ef71cba68 352 }
t_yamamoto 0:669ef71cba68 353 }
t_yamamoto 0:669ef71cba68 354
M_souta 22:7d93f79a3686 355
kishibekairohan 16:3f2c2d89372b 356 //Emergency!
M_souta 22:7d93f79a3686 357 /*
kishibekairohan 17:50dc4b449e69 358 if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan 17:50dc4b449e69 359 buzzer = 0;
kishibekairohan 17:50dc4b449e69 360 BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan 17:50dc4b449e69 361 EMGflag = true;
kishibekairohan 17:50dc4b449e69 362 LED_DEBUG0 = 1;
kishibekairohan 9:f93fc79a49ea 363 }
kishibekairohan 17:50dc4b449e69 364 if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan 17:50dc4b449e69 365 buzzer = 1;
kishibekairohan 9:f93fc79a49ea 366 BuzzerTimer.detach();
kishibekairohan 17:50dc4b449e69 367 EMGflag = false;
kishibekairohan 9:f93fc79a49ea 368 }
M_souta 22:7d93f79a3686 369 */
t_yamamoto 0:669ef71cba68 370 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 371 }
t_yamamoto 0:669ef71cba68 372 }
t_yamamoto 0:669ef71cba68 373
kishibekairohan 2:c015739085d3 374
kishibekairohan 2:c015739085d3 375
kishibekairohan 2:c015739085d3 376
t_yamamoto 0:669ef71cba68 377 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 378 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 379 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 380 static void Process0()
kishibekairohan 10:1295d39fec3a 381 {
M_souta 21:e3b58d675c1c 382 AllActuatorReset();
t_yamamoto 0:669ef71cba68 383 }
t_yamamoto 0:669ef71cba68 384 #endif
t_yamamoto 0:669ef71cba68 385
t_yamamoto 0:669ef71cba68 386 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 387 static void Process1()
t_yamamoto 0:669ef71cba68 388 {
M_souta 22:7d93f79a3686 389
M_souta 22:7d93f79a3686 390 PIDflag = false;
M_souta 22:7d93f79a3686 391
M_souta 21:e3b58d675c1c 392 if(controller->Button.UP) {
M_souta 21:e3b58d675c1c 393 motor[LIFT_LB].dir = FOR;
M_souta 21:e3b58d675c1c 394 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 395 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 396 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 397 } else if(controller->Button.DOWN) {
M_souta 21:e3b58d675c1c 398 motor[LIFT_LB].dir = BACK;
M_souta 21:e3b58d675c1c 399 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 400 motor[LIFT_RB].dir = FOR;
M_souta 21:e3b58d675c1c 401 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 402 } else {
M_souta 21:e3b58d675c1c 403 motor[LIFT_LB].dir = BRAKE;
M_souta 21:e3b58d675c1c 404 motor[LIFT_LB].pwm = 200;
M_souta 21:e3b58d675c1c 405 motor[LIFT_RB].dir = BRAKE;
M_souta 21:e3b58d675c1c 406 motor[LIFT_RB].pwm = 200;
M_souta 21:e3b58d675c1c 407 }
M_souta 22:7d93f79a3686 408
7ka884 4:ba9df71868df 409
M_souta 21:e3b58d675c1c 410 if(controller->Button.X) {
M_souta 21:e3b58d675c1c 411 motor[LIFT_U].dir = FOR;
M_souta 21:e3b58d675c1c 412 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 413 } else if(controller->Button.Y) {
M_souta 21:e3b58d675c1c 414 motor[LIFT_U].dir = BACK;
M_souta 21:e3b58d675c1c 415 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 416 } else {
M_souta 21:e3b58d675c1c 417 motor[LIFT_U].dir = BRAKE;
M_souta 21:e3b58d675c1c 418 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 419 }
M_souta 21:e3b58d675c1c 420
M_souta 21:e3b58d675c1c 421 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
M_souta 21:e3b58d675c1c 422 motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 423 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 424 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 425 motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 426
M_souta 22:7d93f79a3686 427 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 428 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 429 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 22:7d93f79a3686 430 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 21:e3b58d675c1c 431 } else {
M_souta 21:e3b58d675c1c 432 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 433 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 434 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 435 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 436
M_souta 21:e3b58d675c1c 437 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 438 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 439 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 440 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 441 }
t_yamamoto 0:669ef71cba68 442 }
t_yamamoto 0:669ef71cba68 443 #endif
t_yamamoto 0:669ef71cba68 444
t_yamamoto 0:669ef71cba68 445 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 446 static void Process2()
kishibekairohan 13:b6e02d6261d7 447 {
M_souta 22:7d93f79a3686 448 /*
M_souta 22:7d93f79a3686 449 if(moving) {
M_souta 22:7d93f79a3686 450 if(LimiSw::IsPressed(LSW_LB)) {
M_souta 22:7d93f79a3686 451 if(switchFlag_LB) {
M_souta 22:7d93f79a3686 452 switchFlag_LB = false;
M_souta 22:7d93f79a3686 453 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 454 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 455 } else {
M_souta 22:7d93f79a3686 456 seitchFlag_LB = true;
M_souta 22:7d93f79a3686 457 }
M_souta 22:7d93f79a3686 458 }
M_souta 22:7d93f79a3686 459 if(LimiSw::IsPressed(LSW_RB)) {
M_souta 22:7d93f79a3686 460 if(switchFlag_RB) {
M_souta 22:7d93f79a3686 461 switchFlag_RB = false;
M_souta 22:7d93f79a3686 462 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 463 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 464 } else {
M_souta 22:7d93f79a3686 465 seitchFlag_RB = true;
M_souta 22:7d93f79a3686 466 }
M_souta 22:7d93f79a3686 467 }
M_souta 22:7d93f79a3686 468 if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
M_souta 22:7d93f79a3686 469 } else {
M_souta 22:7d93f79a3686 470 if(controller->Button.UP) {
M_souta 22:7d93f79a3686 471 if(!(state == UPPER)) {
M_souta 22:7d93f79a3686 472 state++;
M_souta 22:7d93f79a3686 473 motor[LIFT_LB].dir = BACK;
M_souta 22:7d93f79a3686 474 motor[LIFT_RB].dir = FOR;
M_souta 22:7d93f79a3686 475 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 476 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 477 }
M_souta 22:7d93f79a3686 478 } else if(controller->Button.DOWN) {
M_souta 22:7d93f79a3686 479 if(!(state == LOWER)) {
M_souta 22:7d93f79a3686 480 state--;
M_souta 22:7d93f79a3686 481 moving = true;
M_souta 22:7d93f79a3686 482 motor[LIFT_LB].dir = FOR;
M_souta 22:7d93f79a3686 483 motor[LIFT_RB].dir = BACK;
M_souta 22:7d93f79a3686 484 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 485 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 486 }
M_souta 22:7d93f79a3686 487 } else {
M_souta 22:7d93f79a3686 488 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 489 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 490 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 491 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 492 }
M_souta 22:7d93f79a3686 493 }
M_souta 22:7d93f79a3686 494 */
kishibekairohan 13:b6e02d6261d7 495 }
kishibekairohan 13:b6e02d6261d7 496 #endif
kishibekairohan 13:b6e02d6261d7 497
kishibekairohan 13:b6e02d6261d7 498 #if USE_PROCESS_NUM>3
kishibekairohan 16:3f2c2d89372b 499 static void Process3()
kishibekairohan 13:b6e02d6261d7 500 {
M_souta 22:7d93f79a3686 501 AllActuatorReset();
kishibekairohan 13:b6e02d6261d7 502 }
kishibekairohan 13:b6e02d6261d7 503 #endif
kishibekairohan 13:b6e02d6261d7 504
kishibekairohan 13:b6e02d6261d7 505 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 506 static void Process4()
M_souta 22:7d93f79a3686 507 {
M_souta 22:7d93f79a3686 508
M_souta 22:7d93f79a3686 509 static int x,y;
M_souta 22:7d93f79a3686 510 static int count = 0;
M_souta 22:7d93f79a3686 511
M_souta 22:7d93f79a3686 512 linePara_U = lineCast(LineHub::GetPara(0));
M_souta 22:7d93f79a3686 513 linePara_B = lineCast(LineHub::GetPara(2));
M_souta 22:7d93f79a3686 514 linePara_L = lineCast(LineHub::GetPara(3));
M_souta 22:7d93f79a3686 515 linePara_R = lineCast(LineHub::GetPara(4));
M_souta 22:7d93f79a3686 516
M_souta 22:7d93f79a3686 517 if(linePara_U == 'A' && count == 0) {
M_souta 22:7d93f79a3686 518 lineFase = 1;
M_souta 22:7d93f79a3686 519 }
kishibekairohan 15:dfcec98f5aa9 520
M_souta 22:7d93f79a3686 521 if(lineFase == 0) {
M_souta 22:7d93f79a3686 522 pw = 0.55;
M_souta 22:7d93f79a3686 523 switch(linePara_U) {
M_souta 22:7d93f79a3686 524 case -2:
M_souta 22:7d93f79a3686 525 x = 5;
M_souta 22:7d93f79a3686 526 y = 3;
M_souta 22:7d93f79a3686 527 break;
M_souta 22:7d93f79a3686 528 case -3:
M_souta 22:7d93f79a3686 529 x = 5;
M_souta 22:7d93f79a3686 530 y = 3;
M_souta 22:7d93f79a3686 531 break;
M_souta 22:7d93f79a3686 532 case -1:
M_souta 22:7d93f79a3686 533 x = 6;
M_souta 22:7d93f79a3686 534 y = 3;
M_souta 22:7d93f79a3686 535 break;
M_souta 22:7d93f79a3686 536 case 0:
M_souta 22:7d93f79a3686 537 x = 7;
M_souta 22:7d93f79a3686 538 y = 3;
M_souta 22:7d93f79a3686 539 break;
M_souta 22:7d93f79a3686 540 case 1:
M_souta 22:7d93f79a3686 541 x = 8;
M_souta 22:7d93f79a3686 542 y = 3;
M_souta 22:7d93f79a3686 543 break;
M_souta 22:7d93f79a3686 544 case 3:
M_souta 22:7d93f79a3686 545 x = 9;
M_souta 22:7d93f79a3686 546 y = 3;
M_souta 22:7d93f79a3686 547 break;
M_souta 22:7d93f79a3686 548 case 2:
M_souta 22:7d93f79a3686 549 x = 9;
M_souta 22:7d93f79a3686 550 y = 3;
M_souta 22:7d93f79a3686 551 break;
M_souta 22:7d93f79a3686 552 case 'A':
M_souta 22:7d93f79a3686 553 lineCheck = true;
M_souta 22:7d93f79a3686 554 x = x;
M_souta 22:7d93f79a3686 555 y = y;
M_souta 22:7d93f79a3686 556 break;
M_souta 22:7d93f79a3686 557 case 'N':
M_souta 22:7d93f79a3686 558 x = 7;
M_souta 22:7d93f79a3686 559 y = 7;
M_souta 22:7d93f79a3686 560 break;
M_souta 22:7d93f79a3686 561 x = 7;
M_souta 22:7d93f79a3686 562 y = 7;
M_souta 22:7d93f79a3686 563 default:
M_souta 22:7d93f79a3686 564 x = 9;
M_souta 22:7d93f79a3686 565 y = 9;
M_souta 22:7d93f79a3686 566 }
M_souta 22:7d93f79a3686 567 if(lineCheck == true && (!(linePara_U) == 'A')) {
M_souta 22:7d93f79a3686 568 count++;
M_souta 22:7d93f79a3686 569 }
M_souta 22:7d93f79a3686 570 } else if(lineFase == 1) {
M_souta 22:7d93f79a3686 571 pw = 0.4;
M_souta 22:7d93f79a3686 572 switch(linePara_B) {
M_souta 22:7d93f79a3686 573 case -2:
M_souta 22:7d93f79a3686 574 x = 5;
M_souta 22:7d93f79a3686 575 y = 3;
M_souta 22:7d93f79a3686 576 break;
M_souta 22:7d93f79a3686 577 case -3:
M_souta 22:7d93f79a3686 578 x = 5;
M_souta 22:7d93f79a3686 579 y = 3;
M_souta 22:7d93f79a3686 580 break;
M_souta 22:7d93f79a3686 581 case -1:
M_souta 22:7d93f79a3686 582 x = 6;
M_souta 22:7d93f79a3686 583 y = 3;
M_souta 22:7d93f79a3686 584 break;
M_souta 22:7d93f79a3686 585 case 0:
M_souta 22:7d93f79a3686 586 x = 7;
M_souta 22:7d93f79a3686 587 y = 3;
M_souta 22:7d93f79a3686 588 break;
M_souta 22:7d93f79a3686 589 case 1:
M_souta 22:7d93f79a3686 590 x = 8;
M_souta 22:7d93f79a3686 591 y = 3;
M_souta 22:7d93f79a3686 592 break;
M_souta 22:7d93f79a3686 593 case 3:
M_souta 22:7d93f79a3686 594 x = 9;
M_souta 22:7d93f79a3686 595 y = 3;
M_souta 22:7d93f79a3686 596 break;
M_souta 22:7d93f79a3686 597 case 2:
M_souta 22:7d93f79a3686 598 x = 9;
M_souta 22:7d93f79a3686 599 y = 3;
M_souta 22:7d93f79a3686 600 break;
M_souta 22:7d93f79a3686 601 case 'A':
M_souta 22:7d93f79a3686 602 x = 7;
M_souta 22:7d93f79a3686 603 y = 7;
M_souta 22:7d93f79a3686 604 break;
M_souta 22:7d93f79a3686 605 case 'N':
M_souta 22:7d93f79a3686 606 x = 7;
M_souta 22:7d93f79a3686 607 y = 7;
M_souta 22:7d93f79a3686 608 break;
M_souta 22:7d93f79a3686 609 x = 7;
M_souta 22:7d93f79a3686 610 y = 7;
M_souta 22:7d93f79a3686 611 default:
M_souta 22:7d93f79a3686 612 x = 9;
M_souta 22:7d93f79a3686 613 y = 9;
M_souta 22:7d93f79a3686 614 }
M_souta 22:7d93f79a3686 615 if(linePara_R == 0 && linePara_L == 0) {
M_souta 22:7d93f79a3686 616 lineFase = 2;
M_souta 22:7d93f79a3686 617 x = 7;
M_souta 22:7d93f79a3686 618 y = 7;
M_souta 22:7d93f79a3686 619 }
M_souta 22:7d93f79a3686 620 } else if(lineFase == 2) {
M_souta 22:7d93f79a3686 621 x = 7;
M_souta 22:7d93f79a3686 622 y = 7;
M_souta 22:7d93f79a3686 623 } else {
M_souta 22:7d93f79a3686 624 x = 7;
M_souta 22:7d93f79a3686 625 y = 7;
M_souta 22:7d93f79a3686 626 }
M_souta 22:7d93f79a3686 627
M_souta 22:7d93f79a3686 628 int t = 0;
M_souta 22:7d93f79a3686 629 if((linePara_U + linePara_B) > 3) t = 1;
M_souta 22:7d93f79a3686 630
M_souta 22:7d93f79a3686 631 if(controller->Button.A) {
M_souta 22:7d93f79a3686 632 motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] );
M_souta 22:7d93f79a3686 633 motor[TIRE_FL].dir = SetStatus(omni[y][x]);
M_souta 22:7d93f79a3686 634 motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]);
M_souta 22:7d93f79a3686 635 motor[TIRE_FR].dir = SetStatus(omni[x][14-y]);
M_souta 22:7d93f79a3686 636
M_souta 22:7d93f79a3686 637 motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw;
M_souta 22:7d93f79a3686 638 motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw;
M_souta 22:7d93f79a3686 639 motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw;
M_souta 22:7d93f79a3686 640 motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw;
M_souta 22:7d93f79a3686 641 } else {
M_souta 22:7d93f79a3686 642 motor[TIRE_BL].dir = SetStatus(0);
M_souta 22:7d93f79a3686 643 motor[TIRE_FL].dir = SetStatus(0);
M_souta 22:7d93f79a3686 644 motor[TIRE_BR].dir = SetStatus(0);
M_souta 22:7d93f79a3686 645 motor[TIRE_FR].dir = SetStatus(0);
M_souta 22:7d93f79a3686 646
M_souta 22:7d93f79a3686 647 motor[TIRE_FR].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 648 motor[TIRE_FL].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 649 motor[TIRE_BR].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 650 motor[TIRE_BL].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 651 }
M_souta 22:7d93f79a3686 652
kishibekairohan 13:b6e02d6261d7 653 }
kishibekairohan 13:b6e02d6261d7 654 #endif
kishibekairohan 13:b6e02d6261d7 655
kishibekairohan 13:b6e02d6261d7 656 #if USE_PROCESS_NUM>5
kishibekairohan 16:3f2c2d89372b 657 static void Process5()
kishibekairohan 16:3f2c2d89372b 658 {
M_souta 22:7d93f79a3686 659 lineFase = 0;
M_souta 22:7d93f79a3686 660 lineCheck = true;
kishibekairohan 13:b6e02d6261d7 661 }
kishibekairohan 13:b6e02d6261d7 662 #endif
kishibekairohan 13:b6e02d6261d7 663
kishibekairohan 13:b6e02d6261d7 664 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 665 static void Process6()
kishibekairohan 13:b6e02d6261d7 666 {
kishibekairohan 13:b6e02d6261d7 667
M_souta 22:7d93f79a3686 668 for(int i = 0; i < 8; i++){
M_souta 22:7d93f79a3686 669 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 22:7d93f79a3686 670 }
M_souta 22:7d93f79a3686 671
M_souta 22:7d93f79a3686 672 static int count = 0;
M_souta 22:7d93f79a3686 673 count++;
M_souta 22:7d93f79a3686 674
M_souta 22:7d93f79a3686 675 if(count < 10000) {
M_souta 22:7d93f79a3686 676 lineCheck = false;
M_souta 22:7d93f79a3686 677 } else {
M_souta 22:7d93f79a3686 678 lineCheck = true;
M_souta 22:7d93f79a3686 679 }
M_souta 22:7d93f79a3686 680
M_souta 22:7d93f79a3686 681 if(lineFase == 0) { // 前進
M_souta 22:7d93f79a3686 682 switch(linePara[0]) {
M_souta 22:7d93f79a3686 683 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 684 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 685 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 686 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 687 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 688 motor[TIRE_FR].pwm = 0;
M_souta 22:7d93f79a3686 689 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 690 motor[TIRE_BL].pwm = 0;
M_souta 22:7d93f79a3686 691 break;
M_souta 22:7d93f79a3686 692 case -3:
M_souta 22:7d93f79a3686 693 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 694 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 695 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 696 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 697 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 698 motor[TIRE_FR].pwm = 10;
M_souta 22:7d93f79a3686 699 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 700 motor[TIRE_BL].pwm = 10;
M_souta 22:7d93f79a3686 701 break;
M_souta 22:7d93f79a3686 702 case -1:
M_souta 22:7d93f79a3686 703 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 704 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 705 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 706 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 707 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 708 motor[TIRE_FR].pwm = 20;
M_souta 22:7d93f79a3686 709 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 710 motor[TIRE_BL].pwm = 20;
M_souta 22:7d93f79a3686 711 break;
M_souta 22:7d93f79a3686 712 case 0:
M_souta 22:7d93f79a3686 713 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 714 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 715 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 716 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 717 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 718 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 719 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 720 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 721 break;
M_souta 22:7d93f79a3686 722 case 1:
M_souta 22:7d93f79a3686 723 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 724 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 725 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 726 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 727 motor[TIRE_FL].pwm = 20;
M_souta 22:7d93f79a3686 728 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 729 motor[TIRE_BR].pwm = 20;
M_souta 22:7d93f79a3686 730 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 731 break;
M_souta 22:7d93f79a3686 732 case 3:
M_souta 22:7d93f79a3686 733 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 734 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 735 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 736 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 737 motor[TIRE_FL].pwm = 10;
M_souta 22:7d93f79a3686 738 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 739 motor[TIRE_BR].pwm = 10;
M_souta 22:7d93f79a3686 740 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 741 break;
M_souta 22:7d93f79a3686 742 case 2:
M_souta 22:7d93f79a3686 743 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 744 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 745 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 746 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 747 motor[TIRE_FL].pwm = 0;
M_souta 22:7d93f79a3686 748 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 749 motor[TIRE_BR].pwm = 0;
M_souta 22:7d93f79a3686 750 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 751 break;
M_souta 22:7d93f79a3686 752 case 'A':
M_souta 22:7d93f79a3686 753 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 754 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 755 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 756 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 757 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 758 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 759 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 760 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 761 if(lineCheck == true) {
M_souta 22:7d93f79a3686 762 lineCount++;
M_souta 22:7d93f79a3686 763 count = 0;
M_souta 22:7d93f79a3686 764 }
M_souta 22:7d93f79a3686 765 default:
M_souta 22:7d93f79a3686 766 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 767 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 768 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 769 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 770 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 771 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 772 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 773 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 774 }
M_souta 22:7d93f79a3686 775 if(lineCount == 1) {
M_souta 22:7d93f79a3686 776 lineFase = 1;
M_souta 22:7d93f79a3686 777 }
M_souta 22:7d93f79a3686 778 } else if(lineFase == 1) { // 前進 低速
M_souta 22:7d93f79a3686 779 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 780 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 781 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 782 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 783 motor[TIRE_FL].pwm = 15;
M_souta 22:7d93f79a3686 784 motor[TIRE_FR].pwm = 15;
M_souta 22:7d93f79a3686 785 motor[TIRE_BR].pwm = 15;
M_souta 22:7d93f79a3686 786 motor[TIRE_BL].pwm = 15;
M_souta 22:7d93f79a3686 787 if(linePara[4] == 0) {
M_souta 22:7d93f79a3686 788 lineFase = 2;
M_souta 22:7d93f79a3686 789 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 790 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 791 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 792 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 793 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 794 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 795 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 796 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 797 }
M_souta 22:7d93f79a3686 798 } else if(lineFase == 2){ // 位置調整
M_souta 22:7d93f79a3686 799 lineFase = 3;
M_souta 22:7d93f79a3686 800 } else if(lineFase == 3){ // 右 直進
M_souta 22:7d93f79a3686 801 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 802 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 803 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 804 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 805 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 806 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 807 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 808 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 809 }
M_souta 22:7d93f79a3686 810
kishibekairohan 13:b6e02d6261d7 811 }
kishibekairohan 13:b6e02d6261d7 812 #endif
kishibekairohan 13:b6e02d6261d7 813
kishibekairohan 13:b6e02d6261d7 814 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 815 static void Process7()
kishibekairohan 13:b6e02d6261d7 816 {
M_souta 22:7d93f79a3686 817 if(lineFase == 0) { // 前進
M_souta 22:7d93f79a3686 818 switch(linePara[0]) {
M_souta 22:7d93f79a3686 819 case -2: adj_F = 10;
M_souta 22:7d93f79a3686 820 break;
M_souta 22:7d93f79a3686 821 case -3: adj_F = 5;
M_souta 22:7d93f79a3686 822 break;
M_souta 22:7d93f79a3686 823 case -1: adj_F = 2;
M_souta 22:7d93f79a3686 824 break;
M_souta 22:7d93f79a3686 825 case 0: adj_F = 0;
M_souta 22:7d93f79a3686 826 break;
M_souta 22:7d93f79a3686 827 case 1: adj_F = -2;
M_souta 22:7d93f79a3686 828 break;
M_souta 22:7d93f79a3686 829 case 3: adj_F = -5;
M_souta 22:7d93f79a3686 830 break;
M_souta 22:7d93f79a3686 831 case 2: adj_F = -10;
M_souta 22:7d93f79a3686 832 break;
M_souta 22:7d93f79a3686 833 case 'A':
M_souta 22:7d93f79a3686 834 break;
M_souta 22:7d93f79a3686 835 case 'N':
M_souta 22:7d93f79a3686 836 break;
M_souta 22:7d93f79a3686 837 default:
M_souta 22:7d93f79a3686 838 }
M_souta 22:7d93f79a3686 839 switch(linePara[2]) {
M_souta 22:7d93f79a3686 840 case -2: adj_F = -10;
M_souta 22:7d93f79a3686 841 break;
M_souta 22:7d93f79a3686 842 case -3: adj_F = -5;
M_souta 22:7d93f79a3686 843 break;
M_souta 22:7d93f79a3686 844 case -1: adj_F = -2;
M_souta 22:7d93f79a3686 845 break;
M_souta 22:7d93f79a3686 846 case 0: adj_F = 0;
M_souta 22:7d93f79a3686 847 break;
M_souta 22:7d93f79a3686 848 case 1: adj_F = 2;
M_souta 22:7d93f79a3686 849 break;
M_souta 22:7d93f79a3686 850 case 3: adj_F = 5;
M_souta 22:7d93f79a3686 851 break;
M_souta 22:7d93f79a3686 852 case 2: adj_F = 10;
M_souta 22:7d93f79a3686 853 break;
M_souta 22:7d93f79a3686 854 case 'A': adj_F = 0;
M_souta 22:7d93f79a3686 855 break;
M_souta 22:7d93f79a3686 856 case 'N':
M_souta 22:7d93f79a3686 857 break;
M_souta 22:7d93f79a3686 858 default:
M_souta 22:7d93f79a3686 859 }
M_souta 22:7d93f79a3686 860
M_souta 22:7d93f79a3686 861 if(mode == 0) linePWM = 40;
M_souta 22:7d93f79a3686 862 else if (mode == 1) linePWM = 20;
M_souta 22:7d93f79a3686 863 else if (mode == 2) linePWM = 0;
M_souta 22:7d93f79a3686 864 else linePWM = 0;
M_souta 22:7d93f79a3686 865
M_souta 22:7d93f79a3686 866 motor[TIRE_FL].dir = SetStatus(linePWM - adj_F);
M_souta 22:7d93f79a3686 867 motor[TIRE_BL].dir = SetStatus(linePWM - adj_B);
M_souta 22:7d93f79a3686 868 motor[TIRE_BR].dir = SetStatus(-linePWM - adj_B);
M_souta 22:7d93f79a3686 869 motor[TIRE_FR].dir = SetStatus(-linePWM - adj_F);
M_souta 22:7d93f79a3686 870 motor[TIRE_FR].pwm = SetPWM(linePWM - adj_F);
M_souta 22:7d93f79a3686 871 motor[TIRE_FL].pwm = SetPWM(linePWM - adj_B);
M_souta 22:7d93f79a3686 872 motor[TIRE_BR].pwm = SetPWM(-linePWM - adj_B);
M_souta 22:7d93f79a3686 873 motor[TIRE_BL].pwm = SetPWM(-linePWM - adj_F);
M_souta 22:7d93f79a3686 874 }
kishibekairohan 12:c09b3e08a316 875 }
kishibekairohan 12:c09b3e08a316 876 #endif
kishibekairohan 12:c09b3e08a316 877
kishibekairohan 16:3f2c2d89372b 878 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 879 static void Process8()
kishibekairohan 12:c09b3e08a316 880 {
M_souta 22:7d93f79a3686 881 if(controller->Button.A) {
M_souta 22:7d93f79a3686 882 rotaconSampling.start();
M_souta 22:7d93f79a3686 883 PIDflag = true;
M_souta 22:7d93f79a3686 884
M_souta 22:7d93f79a3686 885 //linePara_U = LineHub::GetPara(0);
M_souta 22:7d93f79a3686 886 //linePara_B = LineHub::GetPara(3);
M_souta 22:7d93f79a3686 887
M_souta 22:7d93f79a3686 888
M_souta 22:7d93f79a3686 889 pulsePV[FL] = encoder[FL].getPulses();
M_souta 22:7d93f79a3686 890 pulsePV[BL] = encoder[BL].getPulses();
M_souta 22:7d93f79a3686 891 pulsePV[BR] = encoder[BR].getPulses();
M_souta 22:7d93f79a3686 892 pulsePV[FR] = encoder[FR].getPulses();
M_souta 22:7d93f79a3686 893
M_souta 22:7d93f79a3686 894
M_souta 22:7d93f79a3686 895 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 896 timeCV[i] = timePV[i];
M_souta 22:7d93f79a3686 897 timePV[i] = rotaconSampling.read();
M_souta 22:7d93f79a3686 898 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta 22:7d93f79a3686 899 pulseCV[i] = pulsePV[i];
M_souta 22:7d93f79a3686 900 }
M_souta 22:7d93f79a3686 901
M_souta 22:7d93f79a3686 902 move.Vx = 0.5;
M_souta 22:7d93f79a3686 903 move.Vy = 0.5;
M_souta 22:7d93f79a3686 904 move.Va = 0;
M_souta 22:7d93f79a3686 905
M_souta 22:7d93f79a3686 906 correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta 22:7d93f79a3686 907 correction_LT.Vy = 0;
M_souta 22:7d93f79a3686 908 correction_LT.Va = 0;
M_souta 22:7d93f79a3686 909
M_souta 22:7d93f79a3686 910 synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta 22:7d93f79a3686 911 synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta 22:7d93f79a3686 912 synthetic.Va = move.Va + correction_LT.Va;
M_souta 22:7d93f79a3686 913
M_souta 22:7d93f79a3686 914 sita = 0;
M_souta 22:7d93f79a3686 915
M_souta 22:7d93f79a3686 916 //タイヤの目標速度算出
M_souta 22:7d93f79a3686 917 float sinR = 0.7071 * (float)sin(sita);
M_souta 22:7d93f79a3686 918 float cosR = 0.7071 * (float)cos(sita);
M_souta 22:7d93f79a3686 919 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta 22:7d93f79a3686 920 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 921 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 922 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 923 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 924
M_souta 22:7d93f79a3686 925 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta 22:7d93f79a3686 926
M_souta 22:7d93f79a3686 927 //PIDによるPWM算出
M_souta 22:7d93f79a3686 928
M_souta 22:7d93f79a3686 929 //モータの駆動
M_souta 22:7d93f79a3686 930 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 931 if (tirePWM[i] > 255){
M_souta 22:7d93f79a3686 932 tirePWM[i] = 255;
M_souta 22:7d93f79a3686 933 } else if (tirePWM[i] < -255) {
M_souta 22:7d93f79a3686 934 tirePWM[i] = -255;
M_souta 22:7d93f79a3686 935 }
M_souta 22:7d93f79a3686 936 }
M_souta 22:7d93f79a3686 937
M_souta 22:7d93f79a3686 938 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 939 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 940 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 941 }
M_souta 22:7d93f79a3686 942 } else {
M_souta 22:7d93f79a3686 943 PIDflag = false;
M_souta 22:7d93f79a3686 944 rotaconSampling.stop();
M_souta 22:7d93f79a3686 945 rotaconSampling.reset();
M_souta 22:7d93f79a3686 946 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 947 encoder[i].reset();
M_souta 22:7d93f79a3686 948 pulsePV[i] = 0;
M_souta 22:7d93f79a3686 949 pulseCV[i] = 0;
M_souta 22:7d93f79a3686 950 timePV[i] = 0;
M_souta 22:7d93f79a3686 951 timeCV[i] = 0;
M_souta 22:7d93f79a3686 952 tirePWM[i] = 0;
M_souta 22:7d93f79a3686 953 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 954 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 955 }
M_souta 22:7d93f79a3686 956 }
kishibekairohan 12:c09b3e08a316 957 }
kishibekairohan 12:c09b3e08a316 958 #endif
kishibekairohan 12:c09b3e08a316 959
kishibekairohan 13:b6e02d6261d7 960 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 961 static void Process9()
kishibekairohan 12:c09b3e08a316 962 {
kishibekairohan 2:c015739085d3 963
t_yamamoto 0:669ef71cba68 964 }
t_yamamoto 0:669ef71cba68 965 #endif
t_yamamoto 0:669ef71cba68 966 #endif
t_yamamoto 0:669ef71cba68 967 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 968
t_yamamoto 0:669ef71cba68 969 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 970 {
t_yamamoto 0:669ef71cba68 971
t_yamamoto 0:669ef71cba68 972 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 973 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 974 #endif
t_yamamoto 0:669ef71cba68 975
t_yamamoto 0:669ef71cba68 976 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 977 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 978 {
t_yamamoto 0:669ef71cba68 979 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 980 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 981 }
t_yamamoto 0:669ef71cba68 982 #endif
t_yamamoto 0:669ef71cba68 983 }
t_yamamoto 0:669ef71cba68 984
kishibekairohan 16:3f2c2d89372b 985 void BuzzerTimer_func(){
kishibekairohan 16:3f2c2d89372b 986 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 987 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 988 }
kishibekairohan 11:028a150943b5 989
kishibekairohan 11:028a150943b5 990 void TapeLedEms_func() {
kishibekairohan 16:3f2c2d89372b 991 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 992 }
kishibekairohan 16:3f2c2d89372b 993
kishibekairohan 16:3f2c2d89372b 994 #pragma region USER-DEFINED-FUNCTIONS
M_souta 22:7d93f79a3686 995 void tirePID() {
M_souta 22:7d93f79a3686 996 if(PIDflag == true) {
M_souta 22:7d93f79a3686 997 //加算するPID値の算出
M_souta 22:7d93f79a3686 998 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta 22:7d93f79a3686 999 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta 22:7d93f79a3686 1000 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta 22:7d93f79a3686 1001 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta 22:7d93f79a3686 1002 //PID値の加算
M_souta 22:7d93f79a3686 1003 tirePWM[FL] += rotaconPID[0].GetMV();
M_souta 22:7d93f79a3686 1004 tirePWM[BL] += rotaconPID[1].GetMV();
M_souta 22:7d93f79a3686 1005 tirePWM[FR] += rotaconPID[2].GetMV();
M_souta 22:7d93f79a3686 1006 tirePWM[BR] += rotaconPID[3].GetMV();
M_souta 22:7d93f79a3686 1007 }
M_souta 22:7d93f79a3686 1008 }
kishibekairohan 16:3f2c2d89372b 1009
M_souta 22:7d93f79a3686 1010 int lineCast(char k) {
M_souta 22:7d93f79a3686 1011 int l;
M_souta 22:7d93f79a3686 1012 switch(k) {
M_souta 22:7d93f79a3686 1013 case 255:
M_souta 22:7d93f79a3686 1014 l = -1;
M_souta 22:7d93f79a3686 1015 break;
M_souta 22:7d93f79a3686 1016 case 254:
M_souta 22:7d93f79a3686 1017 l = -2;
M_souta 22:7d93f79a3686 1018 break;
M_souta 22:7d93f79a3686 1019 case 253:
M_souta 22:7d93f79a3686 1020 l = -3;
M_souta 22:7d93f79a3686 1021 break;
M_souta 22:7d93f79a3686 1022 default:
M_souta 22:7d93f79a3686 1023 l = k;
M_souta 22:7d93f79a3686 1024 }
M_souta 22:7d93f79a3686 1025 return l;
M_souta 22:7d93f79a3686 1026 }
kishibekairohan 16:3f2c2d89372b 1027
t_yamamoto 0:669ef71cba68 1028 #pragma endregion