![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
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?
User | Revision | Line number | New 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 |