三浦 颯太
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Revision 11:028a150943b5, committed 2018-10-06
- Comitter:
- kishibekairohan
- Date:
- Sat Oct 06 08:30:58 2018 +0000
- Parent:
- 10:1295d39fec3a
- Child:
- 12:c09b3e08a316
- Commit message:
- ll
Changed in this revision
--- a/Communication/RS485/ActuatorHub/ActuatorHub.cpp Fri Oct 05 12:17:21 2018 +0000
+++ b/Communication/RS485/ActuatorHub/ActuatorHub.cpp Sat Oct 06 08:30:58 2018 +0000
@@ -2,6 +2,7 @@
#include "mbed.h"
#include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
+#include "../../../System/Process/Process.h"
#include "../../../System/Using.h"
static char RS485Send[RS485_BUFFER_SIZE];
@@ -178,8 +179,15 @@
RS485SendBuffer.PutData(disconnect);
}
#endif
+
+ RS485SendBuffer.PutData('*');
+ RS485SendBuffer.PutData(TAPELED_ADDR);
+ RS485SendBuffer.PutData(sendLedData.red);
+ RS485SendBuffer.PutData(sendLedData.green);
+ RS485SendBuffer.PutData(sendLedData.blue);
+ RS485SendBuffer.PutData('\r');
- // __enable_irq();
+ __enable_irq();
}
}
}
--- a/Communication/RS485/ActuatorHub/ActuatorHub.h Fri Oct 05 12:17:21 2018 +0000
+++ b/Communication/RS485/ActuatorHub/ActuatorHub.h Sat Oct 06 08:30:58 2018 +0000
@@ -12,6 +12,7 @@
#define MOTOR3_ADDR 'O'
#define SOLENOID_ADDR 'S'
#define BLUETOOTH_ADDR 'B'
+ #define TAPELED_ADDR 'L'
namespace MOTOR {
#define FREE 0
--- a/System/Process/Process.cpp Fri Oct 05 12:17:21 2018 +0000
+++ b/System/Process/Process.cpp Sat Oct 06 08:30:58 2018 +0000
@@ -42,23 +42,6 @@
Serial pc(USBTX, USBRX);
-unsigned long ColorIn(int index)
-{
- int result = 0;
- bool rtn = false;
- for(int i=0; i<12; i++)
- {
- CK[index] = 1;
- rtn = DOUT[index];
- CK[index] = 0;
- if(rtn)
- {
- result|=(1 << i);
- }
- }
- return result;
-}
-
#define TIRE_FR 0 //足回り前右
#define TIRE_FL 1 //足回り前左
#define TIRE_BR 2 //足回り後右
@@ -73,6 +56,8 @@
#define Lim_L 1 //センター左
#define EMS_0 LimitSw::IsPressed(8)
#define EMS_1 LimitSw::IsPressed(9)
+#define LS LimitSw::IsPressed(7) //赤ゾーン用スイッチ
+#define BS LimitSw::IsPressed(6) //青ゾーン用スイッチ
//************メカナム********************
const int mecanum[15][15]=
@@ -118,6 +103,22 @@
int Color_D[3];
int intergration = 50;
+unsigned long ColorIn(int index)
+{
+ int result = 0;
+ bool rtn = false;
+ for(int i=0; i<12; i++)
+ {
+ CK[index] = 1;
+ rtn = DOUT[index];
+ CK[index] = 0;
+ if(rtn)
+ {
+ result|=(1 << i);
+ }
+ }
+ return result;
+}
void ColorDetection();
//************カラーセンサ********************
@@ -163,21 +164,30 @@
//************ジャイロ*******************
bool Angle_flagI = false;
-float Angle;
+int Angle;
PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
-
float rotateY;
-int AngletargetX = 18;
-int AngletargetY = -35;
+//初期値 -5
+int AngletargetX = 4;
+int AngletargetY = -12;
int Angle_I = -5;
//************ジャイロ*******************
//************Buzzer******************
-DigitalOut buzzer(BUZZER_PIN);
+//DigitalOut buzzer(BUZZER_PIN);
+PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool Emsflag = false;
//************Buzzer******************
+
+//************TapeLed*****************
+void TapeLedEms_func();
+TapeLedData tapeLED;
+TapeLedData sendLedData;
+TapeLED_Mode ledMode = Normal;
+Ticker tapeLedTimer;
+//************TapaLed*****************
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -216,10 +226,10 @@
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
+ /*Replace here with the initialization code of your variables.*/
get_rpm.attach_us(&filip,100);
+ buzzer.period(1.0/800);
- /*Replace here with the initialization code of your variables.*/
-
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
@@ -302,65 +312,6 @@
while(1)
{
- /*get_rpm.attach_us(&filip,1000);
-
- disX = 48*3.141592*rpmX;
- disY = 48*3.141592*rpmY;
-
- RtpwmX = Rt_X.SetPV(disX , goalX);
- RtpwmY = Rt_Y.SetPV(disY , goalY);
-
- if(controller->Button.B){
- Rt_flagX = true;
- }
- Rt_flagY = true;
- if (Rt_flagY){
- motor[TIRE_FR].dir = SetStatus(RtpwmY);
- motor[TIRE_FL].dir = SetStatus(RtpwmY);
- motor[TIRE_BR].dir = SetStatus(-RtpwmY);
- motor[TIRE_BL].dir = SetStatus(-RtpwmY);
- motor[TIRE_FR].pwm = SetPWM(RtpwmY);
- motor[TIRE_FL].pwm = SetPWM(RtpwmY);
- motor[TIRE_BR].pwm = SetPWM(RtpwmY);
- motor[TIRE_BL].pwm = SetPWM(RtpwmY);
- }
- if(goalY - 15 < disY && disY < goalY + 15){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- Rt_flagY = false;
- Rt_flagX = true;
- }
- if(Rt_flagX){
- motor[TIRE_FR].dir = SetStatus(RtpwmX);
- motor[TIRE_FL].dir = SetStatus(-RtpwmX);
- motor[TIRE_BR].dir = SetStatus(RtpwmX);
- motor[TIRE_BL].dir = SetStatus(-RtpwmX);
- motor[TIRE_FR].pwm = SetPWM(RtpwmX);
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- if(goalX - 15 < disX && disX < goalX + 15){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- Rt_flagX = false;
- }else{
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- }
- pc.printf("%f \r\n",RtpwmX);
- wait_ms(50);
-
-
- /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
- else LED_DEBUG0 = LED_OFF;*/
-
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
@@ -389,15 +340,33 @@
}
if ((EMS_0 || EMS_1) && !Emsflag){
- buzzer = 1;
+ buzzer = 0.5;
BuzzerTimer.attach(BuzzerTimer_func, 1.2);
Emsflag = true;
+ ledMode = EMS;
+ current = 0;
+ tapeLedTimer.attach(TapeLedEms_func, 1.2);
+ sendLedData.code = (uint32_t)Red;
}
if(!EMS_0 && !EMS_1) {
buzzer = 0;
BuzzerTimer.detach();
Emsflag = false;
+ if(ledMode == EMS) ledMode = Normal;
+ tapeLedTimer.detach();
+ }
+
+ switch(ledMode)
+ {
+ case EMS :
+ break;
+
+ case Normal :
+ sendLedData.code = tapeLED.code;
+
+ default:
+ break;
}
SystemProcessUpdate();
@@ -455,6 +424,12 @@
}
}
+ else{
+ motor[Angle_R].dir = BRAKE;
+ motor[Angle_L].dir = BRAKE;
+ motor[Angle_R].pwm = 255;
+ motor[Angle_L].pwm = 255;
+ }
}
#endif
@@ -751,7 +726,7 @@
float rotateY = (y - 305)/2.21 - 90;
Angle += rotateY;
}
- Angle = Angle /20;
+ Angle = Angle/20;
pc.printf("Y:%d \r\n",Angle);
}
#endif
@@ -1156,7 +1131,7 @@
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
}
- pc.printf("X:%d \r\n",RtpwmX);
+ //pc.printf("X:%d \r\n",RtpwmX);
}
#endif
@@ -1262,6 +1237,15 @@
Color_D[2] = ColorIn(3);
}
void BuzzerTimer_func() {
- buzzer = !buzzer;
+ if(buzzer == 0.5){
+ buzzer = 0;
+ }
+ else if(buzzer == 0){
+ buzzer = 0.5;
+ }
+}
+
+void TapeLedEms_func() {
+ sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
#pragma endregion
--- a/System/Process/Process.h Fri Oct 05 12:17:21 2018 +0000
+++ b/System/Process/Process.h Sat Oct 06 08:30:58 2018 +0000
@@ -13,6 +13,178 @@
#define ROTATE_PER_REVOLUTIONS 50
+typedef union
+{
+ struct
+ {
+ unsigned int blue : 8;
+ unsigned int green : 8;
+ unsigned int red : 8;
+ unsigned int : 8;
+ };
+ uint32_t code;
+} TapeLedData;
+
+enum TapeLED_Mode
+{
+ EMS,
+ Normal,
+ Launch,
+};
+
+enum TapeLED_Color
+{
+ Black = 0x000000,
+ // Red = 0xff0000,
+ Green = 0x008000,
+ Blue = 0x0000ff,
+ White = 0xffffff,
+ // Orange = 0xffa500,
+ // Yellow = 0xffff00,
+ // Purple = 0x800080,
+ // Cyan = 0x00ffff,
+ // Magenta = 0xff00ff,
+ // Lime = 0x00ff00
+ Dimgray = 0x696969,
+ Gray = 0x808080,
+ DarkGray = 0xa9a9a9,
+ Silver = 0xc0c0c0,
+ LightGray = 0xd3d3d3,
+ Gainsboro = 0xdcdcdc,
+ Whitesmoke = 0xf5f5f5,
+ Snow = 0xfffafa,
+ Ghostwhite = 0xf8f8ff,
+ Floralwhite = 0xfffaf0,
+ Linen = 0xfaf0e6,
+ Antiquewhite = 0xfaebd7,
+ Papayawhip = 0xffefd5,
+ Blanchedalmond = 0xffebcd,
+ Bisque = 0xffe4c4,
+ Moccasin = 0xffe4b5,
+ Navajowhite = 0xffdead,
+ Peachpuff = 0xffdab9,
+ Mistyrose = 0xffe4e1,
+ Lavenderblush = 0xfff0f5,
+ Seashell = 0xfff5ee,
+ Oldlace = 0xfdf5e6,
+ Ivory = 0xfffff0,
+ Honeydew = 0xf0fff0,
+ Mintcream = 0xf5fffa,
+ Azure = 0xf0ffff,
+ Aliceblue = 0xf0f8ff,
+ Lavender = 0xe6e6fa,
+ Lightsteelblue = 0xb0c4de,
+ Lightslategray = 0x778899,
+ Slategray = 0x708090,
+ Steelblue = 0x4682b4,
+ Royalblue = 0x4169e1,
+ Midnightblue = 0x191970,
+ Navy = 0x000080,
+ Darkblue = 0x00008b,
+ Mediumblue = 0x0000cd,
+ Dodgerblue = 0x1e90ff,
+ Cornflowerblue = 0x6495ed,
+ Deepskyblue = 0x00bfff,
+ Lightskyblue = 0x87cefa,
+ Skyblue = 0x87ceeb,
+ Lightblue = 0xadd8e6,
+ Powderblue = 0xb0e0e6,
+ Paleturquoise = 0xafeeee,
+ // Lightcyan = 0xe0ffff,
+ Cyan = 0x00ffff,
+ // Aqua = 0x00ffff,
+ Turquoise = 0x40e0d0,
+ Mediumturquoise = 0x48d1cc,
+ Darkturquoise = 0x00ced1,
+ Lightseagreen = 0x20b2aa,
+ Cabetblue = 0x5f9ea0,
+ Darkcyan = 0x008b8b,
+ Teal = 0x008080,
+ Darkslategray = 0x2f4f4f,
+ Darkgreen = 0x006400,
+ Forestgreen = 0x228b22,
+ Seagreen = 0x2e8b57,
+ Mediumseagreen = 0x3cb371,
+ Mediumaquamarine = 0x66cdaa,
+ Darkseagreen = 0x8fbc8f,
+ Aquamarine = 0x7fffd4,
+ Palegreen = 0x98fb98,
+ Lightgreen = 0x90ee90,
+ Springgreen = 0x00ff7f,
+ Mediumspringgreen = 0x00fa9a,
+ Lawngreen = 0x7cfc00,
+ Chartreuse = 0x7fff00,
+ Greenyellow = 0xadff2f,
+ Lime = 0x00ff00,
+ Limegreen = 0x32cd32,
+ Yellowgreen = 0x9acd32,
+ Darkolivegreen = 0x556b2f,
+ Olivedrab = 0x6b8e23,
+ Olive = 0x808000,
+ Darkkhaki = 0xbdb76b,
+ Palegoldenrod = 0xeee8aa,
+ Cornsilk = 0xfff8dc,
+ Beige = 0xf5f5dc,
+ Lightyellow = 0xffffe0,
+ Lightgoldenrodyellow = 0xfafad2,
+ Lemonchiffon = 0xfffacd,
+ Wheat = 0xf5deb3,
+ Burlywood = 0xdeb887,
+ Tan = 0xd2b48c,
+ Khaki = 0xf0e68c,
+ Yellow = 0xffff00,
+ Gold = 0xffd700,
+ // Orange = 0xffa500,
+ Orange = 0xff2500,
+ Sandybrown = 0xf4a460,
+ Darkorange = 0xff8c00,
+ Goldenrod = 0xdaa520,
+ Peru = 0xcd853f,
+ Darkgoldenrod = 0xb8860d,
+ Chocolate = 0xd2691e,
+ Sienna = 0xa0522d,
+ Saddlebrown = 0x8b4513,
+ Marron = 0x800000,
+ Darkred = 0x8b0000,
+ Brown = 0xa52a2a,
+ Firebrick = 0xb22222,
+ Indeanred = 0xcd5c5c,
+ Rosybrown = 0xbc8f8f,
+ Darksalmon = 0xe9967a,
+ Lightcoral = 0xf08080,
+ Salmon = 0xfa8072,
+ Lightsalmon = 0xffa07a,
+ Coral = 0xff7f50,
+ Tomato = 0xff6347,
+ Orangered = 0xff4500,
+ Red = 0xff0000,
+ Crimson = 0xdc143c,
+ Mediumvioletred = 0xc71585,
+ Deeppink = 0xff1493,
+ Hotpink = 0xff69b4,
+ Palevioletred = 0xdb7093,
+ Pink = 0xffc0cb,
+ Lightpink = 0xffb6c1,
+ Thistle = 0xd8bfd8,
+ Magenta = 0xff00ff,
+ // Fuchsia = 0xff00ff,
+ Violet = 0xee82ee,
+ Plum = 0xdda0dd,
+ Orchid = 0xda70d6,
+ Mediumorchid = 0xba55d3,
+ Darkorchid = 0x9932cc,
+ Darkviolet = 0x9400d3,
+ Darkmagenta = 0x8b008b,
+ Purple = 0x800080,
+ Indigo = 0x4b0082,
+ Darkslateblue = 0x483d8b,
+ Blueviolet = 0x8a2be2,
+ Mediumpurple = 0x9370db,
+ Slateblue = 0x6a5acd,
+ Mediumslateblue = 0x7b68ee
+};
+
+extern TapeLedData sendLedData;
#endif