三浦 颯太
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Revision 21:e3b58d675c1c, committed 2019-09-09
- Comitter:
- M_souta
- Date:
- Mon Sep 09 00:19:28 2019 +0000
- Parent:
- 20:eae8c84f318c
- Child:
- 22:7d93f79a3686
- Commit message:
- u
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/RS485/LineHub/LineHub.cpp Mon Sep 09 00:19:28 2019 +0000
@@ -0,0 +1,33 @@
+#include "mbed.h"
+#include "LineHub.h"
+#include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
+#include "../../../System/Process/Process.h"
+#include "../../../System/Using.h"
+
+static char RS485Line[RS485_BUFFER_LINE];
+RINGBUFFER::RingBuffer RS485LineBuffer = RINGBUFFER::RingBuffer(RS485Line, RS485_BUFFER_LINE);
+
+int lineData[8];
+
+namespace LINEHUB {
+
+
+ void LineHub::Initialize() {
+ for(int i = 0; i < 8; i++) {
+ lineData[i] = 0;
+ }
+
+ }
+
+ void LineHub::Update() {
+ if(RS485LineBuffer.IsFullup()) {
+ for(int i = 0; i < 8; i++) {
+ lineData[i] = (int)RS485LineBuffer.GetData();
+ }
+ }
+ }
+
+ int LineHub::GetPara(int num) {
+ return lineData[num];
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/RS485/LineHub/LineHub.h Mon Sep 09 00:19:28 2019 +0000
@@ -0,0 +1,21 @@
+#ifndef LINEHUB_H_
+#define LINEHUB_H_
+
+#include "mbed.h"
+#include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
+
+extern int lineData[8];
+
+extern RINGBUFFER::RingBuffer RS485LineBuffer;
+
+namespace LINEHUB {
+
+ class LineHub {
+ public:
+ static void Initialize();
+ static void Update();
+ static int GetPara(int num);
+ };
+}
+
+#endif //LINEHUB_H_
\ No newline at end of file
--- a/Communication/RS485/RS485.cpp Tue Aug 27 04:33:08 2019 +0000
+++ b/Communication/RS485/RS485.cpp Mon Sep 09 00:19:28 2019 +0000
@@ -1,29 +1,34 @@
#include "RS485.h"
#include "mbed.h"
#include "ActuatorHub/ActuatorHub.h"
+#include "LineHub/LineHub.h"
#include "../../LED/LED.h"
#include "../../System/Using.h"
namespace RS485 {
+
DigitalOut selectBitT(SELECTBIT_T_PIN);
- DigitalOut selectBitR(SELECTBIT_R_PIN);
+// DigitalOut selectBitR(SELECTBIT_R_PIN);
Serial RS485Uart(RS485UART_TX, RS485UART_RX);
Serial RS485Line(RS485LINE_TX, RS485LINE_RX);
+
+ bool readFase = 0;
+ char buffer[RS485_BUFFER_LINE] = {0};
void Transmit();
void Recieve();
void RS485::Initialize() {
selectBitT = 1; //送信固定
- selectBitR = 0; //受信固定
+// selectBitR = 0; //受信固定
RS485Uart.baud(38400);
RS485Uart.attach(Transmit, Serial::TxIrq); //送信割り込み
- RS485Line.baud(38400);
+ RS485Line.baud(9600);
RS485Line.attach(Recieve, Serial::RxIrq); //受信割り込み
}
void Transmit() {
- __disable_irq();
+// __disable_irq();
static uint8_t count = 0;
RS485Uart.putc(RS485SendBuffer.GetData());
if(count >= 200) {
@@ -37,13 +42,31 @@
}
void Recieve() {
+ __disable_irq();
+ static uint8_t time = 0;
static uint8_t count = 0;
- __disable_irq();
char data = RS485Line.getc();
+ if (data == 'S') {
+ readFase = true;
+ time = 0;
+ } else if(data == 'F') {
+ readFase = false;
+ for(int i = 0; i < 8; i++) {
+ lineData[i] = buffer[i];
+ }
+ time = 0;
+ } else if(readFase) {
+ RS485LineBuffer.PutData(data);
+ buffer[time] = data;
+ time++;
+ } else {
+ readFase = false;
+ time = 0;
+ }
if(count >= 200) {
LED_DEBUG1 = !LED_DEBUG1;
count = 0;
} else count++;
__enable_irq();
}
-}
+}
\ No newline at end of file
--- a/Communication/RS485/RS485.h Tue Aug 27 04:33:08 2019 +0000
+++ b/Communication/RS485/RS485.h Mon Sep 09 00:19:28 2019 +0000
@@ -3,7 +3,7 @@
namespace RS485 {
#define SELECTBIT_T_PIN PB_3
- #define SELECTBIT_R_PIN PA_5
+// #define SELECTBIT_R_PIN PA_5 未接続
#define RS485UART_TX PA_9
#define RS485UART_RX PA_10
--- 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
--- a/System/Process/Process.h Tue Aug 27 04:33:08 2019 +0000
+++ b/System/Process/Process.h Mon Sep 09 00:19:28 2019 +0000
@@ -9,6 +9,24 @@
void SystemProcess();
+#define TIRE_FL 3
+#define TIRE_FR 2
+#define TIRE_BL 1
+#define TIRE_BR 0
+
+#define LIFT_LB 4
+#define LIFT_RB 6
+#define LIFT_U 7
+
+
+
+
+
+
+
+
+
+
typedef union
{
struct
--- a/System/Using.h Tue Aug 27 04:33:08 2019 +0000 +++ b/System/Using.h Mon Sep 09 00:19:28 2019 +0000 @@ -22,8 +22,10 @@ #define USE_RS485 #ifdef USE_RS485 # define RS485_BUFFER_SIZE 26 + BLUETOOTH_BUFFER_SIZE + MOUNTING_MOTOR_NUM-13 +# define RS485_BUFFER_LINE 8 #else # define RS485_BUFFER_SIZE 0 +# define RS485_BUFFER_LINE 0 #endif #define USE_SOLENOID