Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@21:e3b58d675c1c, 2019-09-09 (annotated)

Committer:
M_souta
Date:
Mon Sep 09 00:19:28 2019 +0000
Revision:
21:e3b58d675c1c
Parent:
20:eae8c84f318c
Child:
22:7d93f79a3686
u

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"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 8 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
kishibekairohan 16:3f2c2d89372b 13 #include "../../Input/Encoder/Encoder.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
t_yamamoto 0:669ef71cba68 18 using namespace SWITCH;
7ka884 4:ba9df71868df 19 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 20 using namespace ENCODER;
M_souta 21:e3b58d675c1c 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 24 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static bool lock;
t_yamamoto 0:669ef71cba68 28 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 29 static int current;
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 34 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 35 #endif
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 40
kishibekairohan 2:c015739085d3 41 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 42
kishibekairohan 16:3f2c2d89372b 43 //**************Encoder***************
kishibekairohan 16:3f2c2d89372b 44 const int PerRev = 256;
kishibekairohan 16:3f2c2d89372b 45 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 46 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 47 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 48 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 49 //**************Encoder***************
kishibekairohan 7:e88c5d47a3be 50
kishibekairohan 16:3f2c2d89372b 51 //**************Buzzer****************
kishibekairohan 11:028a150943b5 52 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 53 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 54 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 55 bool EMGflag = false;
kishibekairohan 16:3f2c2d89372b 56 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 57 //**************Buzzer****************
kishibekairohan 11:028a150943b5 58
kishibekairohan 11:028a150943b5 59 //************TapeLed*****************
kishibekairohan 11:028a150943b5 60 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 61 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 62 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 63 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 64 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 65 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 66
M_souta 21:e3b58d675c1c 67 const int omni[15][15] =
M_souta 20:eae8c84f318c 68 {
M_souta 21:e3b58d675c1c 69 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 70 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 71 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta 21:e3b58d675c1c 72 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta 21:e3b58d675c1c 73 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta 21:e3b58d675c1c 74 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta 21:e3b58d675c1c 75 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta 21:e3b58d675c1c 76 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta 21:e3b58d675c1c 77 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta 21:e3b58d675c1c 78 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta 21:e3b58d675c1c 79 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta 21:e3b58d675c1c 80 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta 21:e3b58d675c1c 81 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta 21:e3b58d675c1c 82 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta 21:e3b58d675c1c 83 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 84 };
M_souta 21:e3b58d675c1c 85
M_souta 21:e3b58d675c1c 86 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 87
M_souta 21:e3b58d675c1c 88 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 89 uint8_t SetStatus(int pwmVal) {
M_souta 21:e3b58d675c1c 90 if (pwmVal < 0) return BACK;
M_souta 21:e3b58d675c1c 91 else if (pwmVal > 0) return FOR;
M_souta 21:e3b58d675c1c 92 else if (pwmVal == 0) return BRAKE;
M_souta 21:e3b58d675c1c 93 else return BRAKE;
M_souta 20:eae8c84f318c 94 }
M_souta 21:e3b58d675c1c 95 uint8_t SetPWM(int);
M_souta 21:e3b58d675c1c 96 uint8_t SetPWM(int pwmVal) {
M_souta 21:e3b58d675c1c 97 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 21:e3b58d675c1c 98 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 99 }
M_souta 18:c694bae76e51 100
t_yamamoto 0:669ef71cba68 101 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 102
t_yamamoto 0:669ef71cba68 103 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 104 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 105 static void Process0(void);
t_yamamoto 0:669ef71cba68 106 #endif
t_yamamoto 0:669ef71cba68 107 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 108 static void Process1(void);
t_yamamoto 0:669ef71cba68 109 #endif
t_yamamoto 0:669ef71cba68 110 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 111 static void Process2(void);
t_yamamoto 0:669ef71cba68 112 #endif
t_yamamoto 0:669ef71cba68 113 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 114 static void Process3(void);
t_yamamoto 0:669ef71cba68 115 #endif
t_yamamoto 0:669ef71cba68 116 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 117 static void Process4(void);
t_yamamoto 0:669ef71cba68 118 #endif
t_yamamoto 0:669ef71cba68 119 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 120 static void Process5(void);
t_yamamoto 0:669ef71cba68 121 #endif
t_yamamoto 0:669ef71cba68 122 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 123 static void Process6(void);
t_yamamoto 0:669ef71cba68 124 #endif
t_yamamoto 0:669ef71cba68 125 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 126 static void Process7(void);
t_yamamoto 0:669ef71cba68 127 #endif
t_yamamoto 0:669ef71cba68 128 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 129 static void Process8(void);
t_yamamoto 0:669ef71cba68 130 #endif
t_yamamoto 0:669ef71cba68 131 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 132 static void Process9(void);
t_yamamoto 0:669ef71cba68 133 #endif
t_yamamoto 0:669ef71cba68 134 #endif
t_yamamoto 0:669ef71cba68 135
t_yamamoto 0:669ef71cba68 136 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 137 {
t_yamamoto 0:669ef71cba68 138 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 16:3f2c2d89372b 139 /*Replace here with the initialization code of your variables.*/
M_souta 21:e3b58d675c1c 140
M_souta 21:e3b58d675c1c 141
t_yamamoto 0:669ef71cba68 142 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 143
t_yamamoto 0:669ef71cba68 144 lock = true;
t_yamamoto 0:669ef71cba68 145 processChangeComp = true;
t_yamamoto 0:669ef71cba68 146 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 147
t_yamamoto 0:669ef71cba68 148 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 149 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 150 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 151 #endif
t_yamamoto 0:669ef71cba68 152 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 153 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 154 #endif
t_yamamoto 0:669ef71cba68 155 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 156 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 157 #endif
t_yamamoto 0:669ef71cba68 158 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 159 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 160 #endif
t_yamamoto 0:669ef71cba68 161 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 162 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 163 #endif
t_yamamoto 0:669ef71cba68 164 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 165 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 166 #endif
t_yamamoto 0:669ef71cba68 167 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 168 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 169 #endif
t_yamamoto 0:669ef71cba68 170 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 171 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 172 #endif
t_yamamoto 0:669ef71cba68 173 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 174 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 175 #endif
t_yamamoto 0:669ef71cba68 176 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 177 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 178 #endif
t_yamamoto 0:669ef71cba68 179 #endif
t_yamamoto 0:669ef71cba68 180 }
t_yamamoto 0:669ef71cba68 181
t_yamamoto 0:669ef71cba68 182 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 183 {
t_yamamoto 0:669ef71cba68 184 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 185 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 186
t_yamamoto 0:669ef71cba68 187 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 188 {
t_yamamoto 0:669ef71cba68 189 current++;
t_yamamoto 0:669ef71cba68 190 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 191 processChangeComp = false;
t_yamamoto 0:669ef71cba68 192 }
t_yamamoto 0:669ef71cba68 193 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 194 {
t_yamamoto 0:669ef71cba68 195 current--;
t_yamamoto 0:669ef71cba68 196 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 197 processChangeComp = false;
t_yamamoto 0:669ef71cba68 198 }
t_yamamoto 0:669ef71cba68 199 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 200 #endif
t_yamamoto 0:669ef71cba68 201
t_yamamoto 0:669ef71cba68 202 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 203 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 204 #endif
t_yamamoto 0:669ef71cba68 205
t_yamamoto 0:669ef71cba68 206 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 207 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209
t_yamamoto 0:669ef71cba68 210 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 211 ACTUATORHUB::ActuatorHub::Update();
M_souta 21:e3b58d675c1c 212 //LINEHUB::LineHub::Update();
t_yamamoto 0:669ef71cba68 213 #endif
t_yamamoto 0:669ef71cba68 214
t_yamamoto 0:669ef71cba68 215 }
t_yamamoto 0:669ef71cba68 216
kishibekairohan 2:c015739085d3 217
kishibekairohan 2:c015739085d3 218
t_yamamoto 0:669ef71cba68 219 void SystemProcess()
t_yamamoto 0:669ef71cba68 220 {
t_yamamoto 0:669ef71cba68 221 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 222
t_yamamoto 0:669ef71cba68 223 while(1)
kishibekairohan 15:dfcec98f5aa9 224 {
M_souta 21:e3b58d675c1c 225 int g[8];
M_souta 21:e3b58d675c1c 226 for(int i = 0; i < 8; i++){
M_souta 21:e3b58d675c1c 227 g[i] = LineHub::GetPara(i);
M_souta 21:e3b58d675c1c 228 }
M_souta 21:e3b58d675c1c 229 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 20:eae8c84f318c 230
kishibekairohan 17:50dc4b449e69 231 buzzer.period(1.0/800);
kishibekairohan 15:dfcec98f5aa9 232
t_yamamoto 0:669ef71cba68 233 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 234 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 235 #endif
t_yamamoto 0:669ef71cba68 236
t_yamamoto 0:669ef71cba68 237 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 238 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 239 {
t_yamamoto 0:669ef71cba68 240 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 241 AllActuatorReset();
t_yamamoto 0:669ef71cba68 242 lock = true;
t_yamamoto 0:669ef71cba68 243 }
t_yamamoto 0:669ef71cba68 244 else
t_yamamoto 0:669ef71cba68 245 #endif
t_yamamoto 0:669ef71cba68 246 {
t_yamamoto 0:669ef71cba68 247
t_yamamoto 0:669ef71cba68 248 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 249 if(!lock)
t_yamamoto 0:669ef71cba68 250 {
t_yamamoto 0:669ef71cba68 251 Process[current]();
t_yamamoto 0:669ef71cba68 252 }
t_yamamoto 0:669ef71cba68 253 else
t_yamamoto 0:669ef71cba68 254 #endif
t_yamamoto 0:669ef71cba68 255 {
t_yamamoto 0:669ef71cba68 256 //ロック時の処理
t_yamamoto 0:669ef71cba68 257 }
t_yamamoto 0:669ef71cba68 258 }
t_yamamoto 0:669ef71cba68 259
kishibekairohan 16:3f2c2d89372b 260 //Emergency!
kishibekairohan 17:50dc4b449e69 261 if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan 17:50dc4b449e69 262 buzzer = 0;
kishibekairohan 17:50dc4b449e69 263 BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan 17:50dc4b449e69 264 EMGflag = true;
kishibekairohan 17:50dc4b449e69 265 LED_DEBUG0 = 1;
kishibekairohan 9:f93fc79a49ea 266 }
kishibekairohan 17:50dc4b449e69 267 if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan 17:50dc4b449e69 268 buzzer = 1;
kishibekairohan 9:f93fc79a49ea 269 BuzzerTimer.detach();
kishibekairohan 17:50dc4b449e69 270 EMGflag = false;
kishibekairohan 9:f93fc79a49ea 271 }
t_yamamoto 0:669ef71cba68 272 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 273 }
t_yamamoto 0:669ef71cba68 274 }
t_yamamoto 0:669ef71cba68 275
kishibekairohan 2:c015739085d3 276
kishibekairohan 2:c015739085d3 277
kishibekairohan 2:c015739085d3 278
t_yamamoto 0:669ef71cba68 279 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 280 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 281 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 282 static void Process0()
kishibekairohan 10:1295d39fec3a 283 {
M_souta 21:e3b58d675c1c 284 AllActuatorReset();
t_yamamoto 0:669ef71cba68 285 }
t_yamamoto 0:669ef71cba68 286 #endif
t_yamamoto 0:669ef71cba68 287
t_yamamoto 0:669ef71cba68 288 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 289 static void Process1()
t_yamamoto 0:669ef71cba68 290 {
M_souta 21:e3b58d675c1c 291 /*
M_souta 21:e3b58d675c1c 292 if(controller->Button.UP) {
M_souta 21:e3b58d675c1c 293 motor[LIFT_LB].dir = FOR;
M_souta 21:e3b58d675c1c 294 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 295 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 296 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 297 } else if(controller->Button.DOWN) {
M_souta 21:e3b58d675c1c 298 motor[LIFT_LB].dir = BACK;
M_souta 21:e3b58d675c1c 299 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 300 motor[LIFT_RB].dir = FOR;
M_souta 21:e3b58d675c1c 301 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 302 } else {
M_souta 21:e3b58d675c1c 303 motor[LIFT_LB].dir = BRAKE;
M_souta 21:e3b58d675c1c 304 motor[LIFT_LB].pwm = 200;
M_souta 21:e3b58d675c1c 305 motor[LIFT_RB].dir = BRAKE;
M_souta 21:e3b58d675c1c 306 motor[LIFT_RB].pwm = 200;
M_souta 21:e3b58d675c1c 307 }
M_souta 21:e3b58d675c1c 308 */
7ka884 4:ba9df71868df 309
M_souta 21:e3b58d675c1c 310 if(controller->Button.X) {
M_souta 21:e3b58d675c1c 311 motor[LIFT_U].dir = FOR;
M_souta 21:e3b58d675c1c 312 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 313 } else if(controller->Button.Y) {
M_souta 21:e3b58d675c1c 314 motor[LIFT_U].dir = BACK;
M_souta 21:e3b58d675c1c 315 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 316 } else {
M_souta 21:e3b58d675c1c 317 motor[LIFT_U].dir = BRAKE;
M_souta 21:e3b58d675c1c 318 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 319 }
M_souta 21:e3b58d675c1c 320
M_souta 21:e3b58d675c1c 321 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
M_souta 21:e3b58d675c1c 322 motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 323 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 324 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 325 motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 326
M_souta 21:e3b58d675c1c 327 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ;
M_souta 21:e3b58d675c1c 328 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ;
M_souta 21:e3b58d675c1c 329 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
M_souta 21:e3b58d675c1c 330 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ;
M_souta 21:e3b58d675c1c 331 } else {
M_souta 21:e3b58d675c1c 332 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 333 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 334 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 335 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 336
M_souta 21:e3b58d675c1c 337 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 338 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 339 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 340 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 341 }
t_yamamoto 0:669ef71cba68 342 }
t_yamamoto 0:669ef71cba68 343 #endif
t_yamamoto 0:669ef71cba68 344
t_yamamoto 0:669ef71cba68 345 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 346 static void Process2()
kishibekairohan 13:b6e02d6261d7 347 {
kishibekairohan 13:b6e02d6261d7 348
kishibekairohan 13:b6e02d6261d7 349 }
kishibekairohan 13:b6e02d6261d7 350 #endif
kishibekairohan 13:b6e02d6261d7 351
kishibekairohan 13:b6e02d6261d7 352 #if USE_PROCESS_NUM>3
kishibekairohan 16:3f2c2d89372b 353 static void Process3()
kishibekairohan 13:b6e02d6261d7 354 {
kishibekairohan 15:dfcec98f5aa9 355
kishibekairohan 13:b6e02d6261d7 356 }
kishibekairohan 13:b6e02d6261d7 357 #endif
kishibekairohan 13:b6e02d6261d7 358
kishibekairohan 13:b6e02d6261d7 359 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 360 static void Process4()
kishibekairohan 12:c09b3e08a316 361 {
kishibekairohan 15:dfcec98f5aa9 362
kishibekairohan 13:b6e02d6261d7 363 }
kishibekairohan 13:b6e02d6261d7 364 #endif
kishibekairohan 13:b6e02d6261d7 365
kishibekairohan 13:b6e02d6261d7 366 #if USE_PROCESS_NUM>5
kishibekairohan 16:3f2c2d89372b 367 static void Process5()
kishibekairohan 16:3f2c2d89372b 368 {
kishibekairohan 13:b6e02d6261d7 369
kishibekairohan 13:b6e02d6261d7 370 }
kishibekairohan 13:b6e02d6261d7 371 #endif
kishibekairohan 13:b6e02d6261d7 372
kishibekairohan 13:b6e02d6261d7 373 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 374 static void Process6()
kishibekairohan 13:b6e02d6261d7 375 {
kishibekairohan 13:b6e02d6261d7 376
kishibekairohan 13:b6e02d6261d7 377 }
kishibekairohan 13:b6e02d6261d7 378 #endif
kishibekairohan 13:b6e02d6261d7 379
kishibekairohan 13:b6e02d6261d7 380 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 381 static void Process7()
kishibekairohan 13:b6e02d6261d7 382 {
kishibekairohan 12:c09b3e08a316 383
kishibekairohan 12:c09b3e08a316 384 }
kishibekairohan 12:c09b3e08a316 385 #endif
kishibekairohan 12:c09b3e08a316 386
kishibekairohan 16:3f2c2d89372b 387 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 388 static void Process8()
kishibekairohan 12:c09b3e08a316 389 {
kishibekairohan 12:c09b3e08a316 390
kishibekairohan 12:c09b3e08a316 391 }
kishibekairohan 12:c09b3e08a316 392 #endif
kishibekairohan 12:c09b3e08a316 393
kishibekairohan 13:b6e02d6261d7 394 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 395 static void Process9()
kishibekairohan 12:c09b3e08a316 396 {
kishibekairohan 2:c015739085d3 397
t_yamamoto 0:669ef71cba68 398 }
t_yamamoto 0:669ef71cba68 399 #endif
t_yamamoto 0:669ef71cba68 400 #endif
t_yamamoto 0:669ef71cba68 401 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 402
t_yamamoto 0:669ef71cba68 403 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 404 {
t_yamamoto 0:669ef71cba68 405
t_yamamoto 0:669ef71cba68 406 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 407 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 408 #endif
t_yamamoto 0:669ef71cba68 409
t_yamamoto 0:669ef71cba68 410 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 411 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 412 {
t_yamamoto 0:669ef71cba68 413 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 414 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 415 }
t_yamamoto 0:669ef71cba68 416 #endif
t_yamamoto 0:669ef71cba68 417 }
t_yamamoto 0:669ef71cba68 418
kishibekairohan 16:3f2c2d89372b 419 void BuzzerTimer_func(){
kishibekairohan 16:3f2c2d89372b 420 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 421 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 422 }
kishibekairohan 11:028a150943b5 423
kishibekairohan 11:028a150943b5 424 void TapeLedEms_func() {
kishibekairohan 16:3f2c2d89372b 425 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 426 }
kishibekairohan 16:3f2c2d89372b 427
kishibekairohan 16:3f2c2d89372b 428 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 429
kishibekairohan 16:3f2c2d89372b 430
t_yamamoto 0:669ef71cba68 431 #pragma endregion