Miya Miyagawa
The last version programs
Dependencies: mbed TrapezoidControl Pulse QEI
Diff: System/Process/Process.cpp
- Revision:
- 21:e3b58d675c1c
- Parent:
- 20:eae8c84f318c
- Child:
- 22:7d93f79a3686
--- a/System/Process/Process.cpp Tue Aug 27 04:33:08 2019 +0000
+++ b/System/Process/Process.cpp Mon Sep 09 00:19:28 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];
@@ -62,26 +64,39 @@
Ticker tapeLedTimer;
//************TapaLed*****************
-/*************LineHub****************
-Serial linehubUart(PC_10,PC_11);
-char data[6];
-
-void LineRead()
+const int omni[15][15] =
{
- //__disable_irq();
- if(linehubUart.readable()) {
- for(int i=0; i<=5; i++) {
- data[i] = linehubUart.getc();
- }
- }
- //__enable_irq();
+{ 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] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
+
+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;
}
-
-//*************LineHub****************/
-
-float tireProRPM[4];
-float tireTarRPM[4];
-float tirePWM[4];
+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
@@ -122,6 +137,8 @@
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
+
+
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
@@ -192,6 +209,7 @@
#ifdef USE_RS485
ACTUATORHUB::ActuatorHub::Update();
+ //LINEHUB::LineHub::Update();
#endif
}
@@ -204,15 +222,12 @@
while(1)
{
-
- pc.printf("aaa\n\r");
+ int g[8];
+ for(int i = 0; i < 8; i++){
+ g[i] = LineHub::GetPara(i);
+ }
+ 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]);
- 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
@@ -266,14 +281,64 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
-
+ AllActuatorReset();
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
+ /*
+ if(controller->Button.UP) {
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 180;
+ } else if(controller->Button.DOWN) {
+ motor[LIFT_LB].dir = BACK;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = FOR;
+ motor[LIFT_RB].pwm = 180;
+ } else {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ }
+ */
+ if(controller->Button.X) {
+ motor[LIFT_U].dir = FOR;
+ motor[LIFT_U].pwm = 180;
+ } else if(controller->Button.Y) {
+ motor[LIFT_U].dir = BACK;
+ motor[LIFT_U].pwm = 180;
+ } else {
+ motor[LIFT_U].dir = BRAKE;
+ motor[LIFT_U].pwm = 180;
+ }
+
+ if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
+ motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
+ motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
+ motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
+ motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
+
+ motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ;
+ motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ;
+ motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
+ motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ;
+ } else {
+ motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
+ motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
+ motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
+ motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
+
+ motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
+ motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
+ motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
+ motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
+ }
}
#endif