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;
 }