大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 21:1f1e9c585da8
- Parent:
- 19:96a462583af9
- Child:
- 22:c47f29caded6
diff -r 05080fd09347 -r 1f1e9c585da8 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sun Sep 01 01:33:36 2019 +0000
+++ b/System/Process/Process.cpp Fri Sep 13 01:36:42 2019 +0000
@@ -5,6 +5,7 @@
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
+#include "../../Communication/RS485/LineHub/LineHub.h"
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
@@ -17,6 +18,7 @@
using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
+using namespace LINEHUB;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -51,7 +53,7 @@
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool EMGflag = false;
-PwmOut buzzer(BUZZER_PIN);
+//PWMOut buzzer(BUZZER_PIN);
//**************Buzzer****************
//************TapeLed*****************
@@ -64,7 +66,43 @@
float tireProRPM[4];
float tireTarRPM[4];
-float tirePWM[4];
+float tirepwm[4];
+
+const int omni[15][15] = {
+ { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
+ { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
+ { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
+ { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
+ { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
+ { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
+ { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
+ { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
+ { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
+ { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
+ { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
+ { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
+ { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
+ { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
+ { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
+};
+
+const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
+//{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
+
+uint8_t SetStatus(int);
+uint8_t SetStatus(int pwmVal)
+{
+ if (pwmVal < 0) return BACK;
+ else if (pwmVal > 0) return FOR;
+ else if (pwmVal == 0) return BRAKE;
+ else return BRAKE;
+}
+uint8_t Setpwm(int);
+uint8_t Setpwm(int pwmVal)
+{
+ if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
+ else return abs(pwmVal);
+}
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
@@ -103,213 +141,495 @@
void SystemProcessInitialize()
{
- #pragma region USER-DEFINED_VARIABLE_INIT
- /*Replace here with the initialization code of your variables.*/
- #pragma endregion USER-DEFINED_VARIABLE_INIT
+ #pragma region USER-DEFINED_VARIABLE_INIT
+ /*Replace here with the initialization code of your variables.*/
+ #pragma endregion USER-DEFINED_VARIABLE_INIT
- lock = true;
- processChangeComp = true;
- current = DEFAULT_PROCESS;
+ lock = true;
+ processChangeComp = true;
+ current = DEFAULT_PROCESS;
- #ifdef USE_SUBPROCESS
- #if USE_PROCESS_NUM>0
- Process[0] = Process0;
- #endif
- #if USE_PROCESS_NUM>1
- Process[1] = Process1;
- #endif
- #if USE_PROCESS_NUM>2
- Process[2] = Process2;
- #endif
- #if USE_PROCESS_NUM>3
- Process[3] = Process3;
- #endif
- #if USE_PROCESS_NUM>4
- Process[4] = Process4;
- #endif
- #if USE_PROCESS_NUM>5
- Process[5] = Process5;
- #endif
- #if USE_PROCESS_NUM>6
- Process[6] = Process6;
- #endif
- #if USE_PROCESS_NUM>7
- Process[7] = Process7;
- #endif
- #if USE_PROCESS_NUM>8
- Process[8] = Process8;
- #endif
- #if USE_PROCESS_NUM>9
- Process[9] = Process9;
- #endif
- #endif
+#ifdef USE_SUBPROCESS
+#if USE_PROCESS_NUM>0
+ Process[0] = Process0;
+#endif
+#if USE_PROCESS_NUM>1
+ Process[1] = Process1;
+#endif
+#if USE_PROCESS_NUM>2
+ Process[2] = Process2;
+#endif
+#if USE_PROCESS_NUM>3
+ Process[3] = Process3;
+#endif
+#if USE_PROCESS_NUM>4
+ Process[4] = Process4;
+#endif
+#if USE_PROCESS_NUM>5
+ Process[5] = Process5;
+#endif
+#if USE_PROCESS_NUM>6
+ Process[6] = Process6;
+#endif
+#if USE_PROCESS_NUM>7
+ Process[7] = Process7;
+#endif
+#if USE_PROCESS_NUM>8
+ Process[8] = Process8;
+#endif
+#if USE_PROCESS_NUM>9
+ Process[9] = Process9;
+#endif
+#endif
}
static void SystemProcessUpdate()
{
- #ifdef USE_SUBPROCESS
- if(controller->Button.HOME) lock = false;
-
- if(controller->Button.START && processChangeComp)
- {
- current++;
- if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
- processChangeComp = false;
- }
- else if(controller->Button.SELECT && processChangeComp)
- {
- current--;
- if (current < 0) current = 0;
- processChangeComp = false;
- }
- else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
- #endif
-
- #ifdef USE_MOTOR
- ACTUATORHUB::MOTOR::Motor::Update(motor);
- #endif
-
- #ifdef USE_SOLENOID
- ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
- #endif
+#ifdef USE_SUBPROCESS
+ if(controller->Button.HOME) lock = false;
+
+ if(controller->Button.START && processChangeComp) {
+ current++;
+ if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
+ processChangeComp = false;
+ } else if(controller->Button.SELECT && processChangeComp) {
+ current--;
+ if (current < 0) current = 0;
+ processChangeComp = false;
+ } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
+#endif
+
+#ifdef USE_MOTOR
+ ACTUATORHUB::MOTOR::Motor::Update(motor);
+#endif
+
+#ifdef USE_SOLENOID
+ ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
+#endif
+
+#ifdef USE_RS485
+ ACTUATORHUB::ActuatorHub::Update();
+#endif
+
+}
+
+int g[8];
+
+void SystemProcess()
+{
+ SystemProcessInitialize();
+
+ while(1) {
+ for(int i = 0; i < 8; i++) {
+ g[i] = LineHub::GetPara(i);
+ }
- #ifdef USE_RS485
- ACTUATORHUB::ActuatorHub::Update();
- #endif
-
+#ifdef USE_MU
+ controller = CONTROLLER::Controller::GetData();
+#endif
+
+#ifdef USE_ERRORCHECK
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
+ CONTROLLER::Controller::DataReset();
+ AllActuatorReset();
+ lock = true;
+ } else
+#endif
+ {
+
+#ifdef USE_SUBPROCESS
+ if(!lock) {
+ Process[current]();
+ } else
+#endif
+ {
+ //ロック時の処理
+ }
+ }
+ /*
+ //Emergency!
+ if(!EMG_0 && !EMG_1 && !EMGflag) {
+ buzzer = 0;
+ BuzzerTimer.attach(BuzzerTimer_func, 1);
+ EMGflag = true;
+ LED_DEBUG0 = 1;
+ }
+ if(EMG_0 && EMG_1 && EMGflag) {
+ buzzer = 1;
+ BuzzerTimer.detach();
+ EMGflag = false;
+ }
+ */
+ SystemProcessUpdate();
+ }
}
-void SystemProcess()
-{
- SystemProcessInitialize();
-
- while(1)
- {
- if(LimitSw::IsPressed(10)) {
- LED_DEBUG0 = LED_ON;
- } else {
- LED_DEBUG0 = LED_OFF;
- }
- //printf("%d\r\n",ECD_0.getPulses());
- buzzer.period(1.0/800);
-
- #ifdef USE_MU
- controller = CONTROLLER::Controller::GetData();
- #endif
-
- #ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
- {
- CONTROLLER::Controller::DataReset();
- AllActuatorReset();
- lock = true;
- }
- else
- #endif
- {
-
- #ifdef USE_SUBPROCESS
- if(!lock)
- {
- Process[current]();
- }
- else
- #endif
- {
- //ロック時の処理
- }
- }
-
- //Emergency!
- if(!EMG_0 && !EMG_1 && !EMGflag){
- buzzer = 0;
- BuzzerTimer.attach(BuzzerTimer_func, 1);
- EMGflag = true;
- LED_DEBUG0 = 1;
- }
- if(EMG_0 && EMG_1 && EMGflag){
- buzzer = 1;
- BuzzerTimer.detach();
- EMGflag = false;
- }
- SystemProcessUpdate();
- }
-}
-
-
-
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
-static void Process0()
-{
-
+static void Process0()
+{
+
}
#endif
#if USE_PROCESS_NUM>1
+
+
+bool dz1=true;
+bool dz1i=false;
+bool dz2=true;
+bool dz2i=false;
+bool dz3=true;
+bool dz3i=false;
+bool dz4=true;
+bool dz4i=false;
+
+int mode=0;
+int mode1=0;
+int mode1g0=100000;
+
static void Process1()
{
-
+ mode1g0=100000;
+ mode=0;
+ mode1=0;
+ motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
+ motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
+
+ motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+
+ if(controller->Button.UP||controller->Button.DOWN) {
+
+ if(controller->Button.UP) {
+ motor[MOTOR_0].pwm = 110;
+ motor[MOTOR_0].dir = FOR;
+ }
+ if(controller->Button.DOWN) {
+ motor[MOTOR_0].pwm = 50;
+ motor[MOTOR_0].dir = BACK;
+ }
+ } else {
+ motor[MOTOR_0].pwm = 0;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ if(controller->Button.X) {
+ if(dz1==true) {
+ if(dz1i==false) {
+ solenoid.solenoid1 = SOLENOID_ON;
+ dz1i=true;
+ } else {
+ solenoid.solenoid1 = SOLENOID_OFF;
+ dz1i=false;
+ }
+ dz1=false;
+ }
+ } else {
+ dz1=true;
+ }
+
+ if(controller->Button.Y) {
+ if(dz2==true) {
+ if(dz2i==false) {
+ solenoid.solenoid2 = SOLENOID_ON;
+ dz2i=true;
+ } else {
+ solenoid.solenoid2 = SOLENOID_OFF;
+ dz2i=false;
+ }
+ dz2=false;
+ }
+ } else {
+ dz2=true;
+ }
+
+ if(controller->Button.A) {
+ if(dz3==true) {
+ if(dz3i==false) {
+ solenoid.solenoid3 = SOLENOID_ON;
+ dz3i=true;
+ } else {
+ solenoid.solenoid3 = SOLENOID_OFF;
+ dz3i=false;
+ }
+ dz3=false;
+ }
+ } else {
+ dz3=true;
+ }
+
+ if(controller->Button.B) {
+ if(dz4==true) {
+ if(dz4i==false) {
+ solenoid.solenoid4 = SOLENOID_ON;
+ dz4i=true;
+ } else {
+ solenoid.solenoid4 = SOLENOID_OFF;
+ dz4i=false;
+ }
+ dz4=false;
+ }
+ } else {
+ dz4=true;
+ }
+ /*
+ if(controller->Button.RIGHT){
+ motor[MOTOR_1].dir = FOR;
+ motor[MOTOR_1].pwm = 60;
+ if (ARM_1){
+ motor[MOTOR_1].dir = BRAKE;
+ }
+ }
+ else if(controller->Button.LEFT){
+ motor[MOTOR_1].dir = BACK;
+ motor[MOTOR_1].pwm = 60;
+ if (ARM_0){
+ motor[MOTOR_1].dir = BRAKE;
+ }
+ }
+ */
}
#endif
#if USE_PROCESS_NUM>2
+bool mtc=false;
+
static void Process2()
-{
-
+{
+// 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]);
+ mode1g0++;
+ if(g[0]!=99&&mode==0) {
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ if(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc=true;
+ if(mode1==2) {
+ mode=1;
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ }
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
+ case 0:
+ mtc=false;
+ break;
+ case 255:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
+ } else if(mode==1) {
+ motor[TIRE_FR].pwm = 5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 5;
+ motor[TIRE_BL].dir = FOR;
+ if(g[2]==0) {
+ mode=2;
+ }
+ } else if(mode==2) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ mode1g0++;
+ if(mode1g0>10000&&g[2]==0) {
+ mode=3;
+ }
+ } else if(mode==3) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = BRAKE;
+ } else {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+
}
#endif
#if USE_PROCESS_NUM>3
-static void Process3()
+static void Process3()
{
-
}
#endif
#if USE_PROCESS_NUM>4
-static void Process4()
-{
+static void Process4()
+{
}
#endif
#if USE_PROCESS_NUM>5
-static void Process5()
-{
-
+static void Process5()
+{
+
}
#endif
#if USE_PROCESS_NUM>6
-static void Process6()
+static void Process6()
{
-
+
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+
}
#endif
-#if USE_PROCESS_NUM>8
+#if USE_PROCESS_NUM>8
static void Process8()
{
-
+
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
-
+
}
#endif
#endif
@@ -318,25 +638,27 @@
static void AllActuatorReset()
{
- #ifdef USE_SOLENOID
- solenoid.all = ALL_SOLENOID_OFF;
- #endif
+#ifdef USE_SOLENOID
+ solenoid.all = ALL_SOLENOID_OFF;
+#endif
- #ifdef USE_MOTOR
- for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
- {
- motor[i].dir = FREE;
- motor[i].pwm = 0;
- }
- #endif
+#ifdef USE_MOTOR
+ for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+ motor[i].dir = FREE;
+ motor[i].pwm = 0;
+ }
+#endif
}
-void BuzzerTimer_func(){
+/*
+void BuzzerTimer_func()
+{
buzzer = !buzzer;
//LED_DEBUG0 = !LED_DEBUG0;
}
-
-void TapeLedEms_func() {
+*/
+void TapeLedEms_func()
+{
sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}