Taiki Maruyama
/
MainBoard2019_Auto
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 16:3f2c2d89372b, committed 2019-07-01
- Comitter:
- kishibekairohan
- Date:
- Mon Jul 01 13:00:20 2019 +0000
- Parent:
- 15:dfcec98f5aa9
- Commit message:
- aaa;
Changed in this revision
diff -r dfcec98f5aa9 -r 3f2c2d89372b Communication/Controller/Controller.h
--- a/Communication/Controller/Controller.h Sun Oct 21 02:14:15 2018 +0000
+++ b/Communication/Controller/Controller.h Mon Jul 01 13:00:20 2019 +0000
@@ -4,8 +4,8 @@
#include <stdint.h>
namespace CONTROLLER {
- #define MU_TX PC_6
- #define MU_RX PC_7
+ #define MU_TX PA_0
+ #define MU_RX PA_1
#define CTR_DATA_LENGTH 4
#define CTR_DEFAULT_DATA {0x00, 0x00, 0x77, 0x77}
diff -r dfcec98f5aa9 -r 3f2c2d89372b Communication/Controller/Mu/Mu.h
--- a/Communication/Controller/Mu/Mu.h Sun Oct 21 02:14:15 2018 +0000
+++ b/Communication/Controller/Mu/Mu.h Mon Jul 01 13:00:20 2019 +0000
@@ -5,8 +5,8 @@
#include "mbed.h"
namespace MU {
- #define MU_TX PC_6
- #define MU_RX PC_7
+ #define MU_TX PA_0
+ #define MU_RX PA_1
class Mu {
public:
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/AccelerationSensor/AccelerationSensor.cpp
--- a/Input/AccelerationSensor/AccelerationSensor.cpp Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,10 +0,0 @@
-#include "AccelerationSensor.h"
-#include "mbed.h"
-
-namespace ACCELERATIONSENSOR {
- AnalogIn acc[] = {
- AnalogIn(ACCX_PIN),
- AnalogIn(ACCY_PIN),
- AnalogIn(ACCZ_PIN),
- };
-}
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/AccelerationSensor/AccelerationSensor.h
--- a/Input/AccelerationSensor/AccelerationSensor.h Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,14 +0,0 @@
-#ifndef ACCELERATIONSENSOR_H_
-#define ACCELERATIONSENSOR_H_
-
-#include "mbed.h"
-
-namespace ACCELERATIONSENSOR {
- #define ACCX_PIN PA_0
- #define ACCY_PIN PA_6
- #define ACCZ_PIN PA_5
-
- extern AnalogIn acc[];
-}
-
-#endif
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/ColorSensor/ColorSensor.cpp
--- a/Input/ColorSensor/ColorSensor.cpp Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#include "ColorSensor.h"
-#include "mbed.h"
-
-namespace COLORSENSOR {
- DigitalIn DOUT[] = {
- DigitalIn(DOUT0_PIN),
- DigitalIn(DOUT1_PIN),
- DigitalIn(DOUT2_PIN),
- DigitalIn(DOUT3_PIN),
- };
- DigitalOut CK[] = {
- DigitalOut(CK0_PIN),
- DigitalOut(CK1_PIN),
- DigitalOut(CK2_PIN),
- DigitalOut(CK3_PIN),
- };
- DigitalOut RANGE(RANGE_PIN);
- DigitalOut GATE(GATE_PIN);
-}
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/ColorSensor/ColorSensor.h
--- a/Input/ColorSensor/ColorSensor.h Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#ifndef COLORSENSOR_H_
-#define COLORSENSOR_H_
-
-#include "mbed.h"
-
-namespace COLORSENSOR {
- #define DOUT0_PIN PB_1
- #define DOUT1_PIN PC_5
- #define DOUT2_PIN PA_7
- #define DOUT3_PIN PC_4
-
- extern DigitalIn DOUT[];
-
- #define CK0_PIN PB_12
- #define CK1_PIN PA_12
- #define CK2_PIN PB_13
- #define CK3_PIN PB_14
-
- extern DigitalOut CK[];
-
- #define RANGE_PIN PA_11
- extern DigitalOut RANGE;
-
- #define GATE_PIN PB_15
- extern DigitalOut GATE;
-
-}
-
-#endif
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Encoder/Encoder.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Encoder/Encoder.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -0,0 +1,30 @@
+#include "Encoder.h"
+#include "mbed.h"
+
+#include "../../System/Process/InterruptProcess.h"
+
+InterruptIn BoardECD[] = {
+ InterruptIn(ECD_A_0),
+ InterruptIn(ECD_A_1),
+ InterruptIn(ECD_A_2),
+ InterruptIn(ECD_A_3),
+
+ InterruptIn(ECD_B_0),
+ InterruptIn(ECD_B_1),
+ InterruptIn(ECD_B_2),
+ InterruptIn(ECD_B_3),
+};
+
+namespace ENCODER {
+ void ECD::Initialize() {
+ BoardECD[0].mode(PullUp);
+ BoardECD[1].mode(PullUp);
+ BoardECD[2].mode(PullUp);
+ BoardECD[3].mode(PullUp);
+
+ BoardECD[0].fall(int2);
+ BoardECD[1].fall(int3);
+ BoardECD[2].fall(int4);
+ BoardECD[3].fall(int5);
+ }
+}
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Encoder/Encoder.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Encoder/Encoder.h Mon Jul 01 13:00:20 2019 +0000
@@ -0,0 +1,21 @@
+#ifndef ENCODER_H_
+#define ENCODER_H_
+
+namespace ENCODER {
+ #define ECD_A_0 PB_1
+ #define ECD_A_1 PB_12
+ #define ECD_A_2 PC_4
+ #define ECD_A_3 PB_14
+
+ #define ECD_B_0 PB_2
+ #define ECD_B_1 PA_11
+ #define ECD_B_2 PB_13
+ #define ECD_B_3 PB_15
+
+ class ECD {
+ public:
+ static void Initialize();
+ };
+}
+
+#endif
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/ExternalInt/ExternalInt.cpp
--- a/Input/ExternalInt/ExternalInt.cpp Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/ExternalInt/ExternalInt.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -6,14 +6,26 @@
InterruptIn BoardInt[] = {
InterruptIn(INT0_PIN),
InterruptIn(INT1_PIN),
+ InterruptIn(INT2_PIN),
+ InterruptIn(INT3_PIN),
+ InterruptIn(INT4_PIN),
+ InterruptIn(INT5_PIN),
};
namespace EXTERNALINT {
void Int::Initialize() {
BoardInt[0].mode(PullUp);
BoardInt[1].mode(PullUp);
-
+ BoardInt[2].mode(PullUp);
+ BoardInt[3].mode(PullUp);
+ BoardInt[4].mode(PullUp);
+ BoardInt[5].mode(PullUp);
+
BoardInt[0].fall(int0);
BoardInt[1].fall(int1);
+ BoardInt[2].fall(int1);
+ BoardInt[3].fall(int1);
+ BoardInt[4].fall(int1);
+ BoardInt[5].fall(int1);
}
}
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/ExternalInt/ExternalInt.h
--- a/Input/ExternalInt/ExternalInt.h Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/ExternalInt/ExternalInt.h Mon Jul 01 13:00:20 2019 +0000
@@ -2,9 +2,13 @@
#define EXTERNALINT_H_
namespace EXTERNALINT {
- #define INT0_PIN PB_3
- #define INT1_PIN PB_2
-
+ #define INT0_PIN PB_8
+ #define INT1_PIN PC_9
+ #define INT2_PIN PC_8
+ #define INT3_PIN PC_6
+ #define INT4_PIN PC_5
+ #define INT5_PIN PA_12
+
class Int {
public:
static void Initialize();
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Potentiometer/Potentiometer.cpp
--- a/Input/Potentiometer/Potentiometer.cpp Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/Potentiometer/Potentiometer.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -6,5 +6,8 @@
AnalogIn(ADC0_PIN),
AnalogIn(ADC1_PIN),
AnalogIn(ADC2_PIN),
+ AnalogIn(ADC3_PIN),
+ AnalogIn(ADC4_PIN),
+
};
}
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Potentiometer/Potentiometer.h
--- a/Input/Potentiometer/Potentiometer.h Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/Potentiometer/Potentiometer.h Mon Jul 01 13:00:20 2019 +0000
@@ -4,10 +4,12 @@
#include "mbed.h"
namespace POTENTIOMETER {
- #define ADC0_PIN PC_3
- #define ADC1_PIN PC_0
- #define ADC2_PIN PC_1
-
+ #define ADC0_PIN PC_3
+ #define ADC1_PIN PC_0
+ #define ADC2_PIN PC_1
+ #define ADC3_PIN PC_0
+ #define ADC4_PIN PB_0
+
extern AnalogIn adc[];
}
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Rotaryencoder/Rotaryencoder.cpp
--- a/Input/Rotaryencoder/Rotaryencoder.cpp Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,26 +0,0 @@
-#include "Rotaryencoder.h"
-#include "mbed.h"
-
-#include "../../System/Process/InterruptProcess.h"
-
-InterruptIn BoardRt[] = {
- InterruptIn(RT11_PIN),
- InterruptIn(RT12_PIN),
-
- InterruptIn(RT21_PIN),
- InterruptIn(RT22_PIN),
-};
-
-namespace ROTARYENCODER {
- void Rot::Initialize() {
- BoardRt[0].mode(PullUp);
- BoardRt[1].mode(PullUp);
- BoardRt[2].mode(PullUp);
- BoardRt[3].mode(PullUp);
-
- BoardRt[0].fall(int2);
- BoardRt[1].fall(int3);
- BoardRt[2].fall(int4);
- BoardRt[3].fall(int5);
- }
-}
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Rotaryencoder/Rotaryencoder.h
--- a/Input/Rotaryencoder/Rotaryencoder.h Sun Oct 21 02:14:15 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,17 +0,0 @@
-#ifndef ROTARYENCODER_H_
-#define ROTARYENCODER_H_
-
-namespace ROTARYENCODER {
- #define RT11_PIN PB_0
- #define RT12_PIN PA_4
-
- #define RT21_PIN PC_2
- #define RT22_PIN PA_1
-
- class Rot {
- public:
- static void Initialize();
- };
-}
-
-#endif
\ No newline at end of file
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Switch/Switch.cpp
--- a/Input/Switch/Switch.cpp Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/Switch/Switch.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -41,43 +41,58 @@
}
bool LimitSw::IsPressed(int index) {
- // if(index > 0x0f) return false;
-
- //printf("%d\n", index);
MP_Channel ch;
switch(index){
case 0:
- ch.all = 10;
+ ch.all = 8;
break;
case 1:
- ch.all = 11;
+ ch.all = 9;
break;
case 2:
- ch.all = 8;
+ ch.all = 10;
break;
case 3:
- ch.all = 9;
+ ch.all = 11;
break;
case 4:
+ ch.all = 12;
+ break;
+ case 5:
+ ch.all = 13;
+ break;
+ case 6:
+ ch.all = 14;
+ break;
+ case 7:
+ ch.all = 15;
+ break;
+ case 8:
+ ch.all = 7;
+ break;
+ case 9:
ch.all = 6;
break;
- case 5:
- ch.all = 7;
- break;
- case 6:
+ case 10:
ch.all = 4;
break;
- case 7:
+ case 11:
ch.all = 5;
break;
- case 8:
+ case 12:
ch.all = 2;
break;
- case 9:
+ case 13:
ch.all = 3;
break;
+ case 14:
+ ch.all = 0;
+ break;
+ case 15:
+ ch.all = 1;
+ break;
}
diff -r dfcec98f5aa9 -r 3f2c2d89372b Input/Switch/Switch.h
--- a/Input/Switch/Switch.h Sun Oct 21 02:14:15 2018 +0000
+++ b/Input/Switch/Switch.h Mon Jul 01 13:00:20 2019 +0000
@@ -8,20 +8,21 @@
#define SW_ON 0
#define SW_OFF 1
-
- #define DIP0_PIN PA_8
- #define DIP1_PIN PB_10
- #define DIP2_PIN PB_4
- #define DIP3_PIN PB_5
+ //DipSwitch
+ #define DIP0_PIN PA_4
+ #define DIP1_PIN PA_6
+ #define DIP2_PIN PA_7
+ #define DIP3_PIN PC_7
+
- //マルチプレクサ
- #define LS_PIN PB_6
- #define SELECT0_PIN PC_8
- #define SELECT1_PIN PC_9
- #define SELECT2_PIN PB_9
- #define SELECT3_PIN PB_8
-
- //Dipsw
+ //Multiplexer
+ #define LS_PIN PD_2
+ #define SELECT0_PIN PC_12
+ #define SELECT1_PIN PA_15
+ #define SELECT2_PIN PC_13
+ #define SELECT3_PIN PB_7
+
+
#define DIP0 dipSw[0]
#define DIP1 dipSw[1]
#define DIP2 dipSw[2]
diff -r dfcec98f5aa9 -r 3f2c2d89372b LED/LED.h
--- a/LED/LED.h Sun Oct 21 02:14:15 2018 +0000
+++ b/LED/LED.h Mon Jul 01 13:00:20 2019 +0000
@@ -10,10 +10,10 @@
#define LED_ON 0
#define LED_OFF 1
- #define LED_DEBUG0_PIN PC_11
- #define LED_DEBUG1_PIN PC_10
- #define LED_DEBUG2_PIN PC_12
- #define LED_MU_PIN PC_13
+ #define LED_DEBUG0_PIN PA_8
+ #define LED_DEBUG1_PIN PB_10
+ #define LED_DEBUG2_PIN PB_4
+ #define LED_MU_PIN PB_5
#define LED_DEBUG0 LED::boardLED[0]
#define LED_DEBUG1 LED::boardLED[1]
diff -r dfcec98f5aa9 -r 3f2c2d89372b System/Process/Process.cpp
--- a/System/Process/Process.cpp Sun Oct 21 02:14:15 2018 +0000
+++ b/System/Process/Process.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -8,20 +8,15 @@
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
-#include "../../Input/ColorSensor/ColorSensor.h"
-#include "../../Input/AccelerationSensor/AccelerationSensor.h"
#include "../../Input/Potentiometer/Potentiometer.h"
-#include "../../Input/Rotaryencoder/Rotaryencoder.h"
+#include "../../Input/Encoder/Encoder.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
-
using namespace SWITCH;
-using namespace COLORSENSOR;
-using namespace ACCELERATIONSENSOR;
using namespace PID_SPACE;
-using namespace ROTARYENCODER;
+using namespace ENCODER;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -43,134 +38,22 @@
Serial pc(USBTX, USBRX);
-//************メカナム********************
-
-const int mecanum[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] = {-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;
-}
-uint8_t SetPWM(int);
-uint8_t SetPWM(int pwmVal){
- if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
- else return abs(pwmVal);
-}
-
-//************メカナム********************
-
-//************カラーセンサ********************
-
-int Color_A[3]; //[赤,緑,青]
-int Color_B[3];
-int Color_C[3];
-int Color_D[3];
-int intergration = 50;
-
-int Avecolor_A[3];
-int Avecolor_B[3];
-int Avecolor_C[3];
-int Avecolor_D[3];
-
-void ColorIn();
-void ColorDetection();
-void getcolor();
-
-//************カラーセンサ********************
+//**************Encoder***************
+const int PerRev = 256;
+QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
+//**************Encoder***************
-//************ライントレース変数*******************
-int PointA[3] = {400, 700, 1000};//赤，緑，青
-int PointB[3] = {400, 700, 1000};//赤，緑，青
-int PointC[3] = {1000, 1700, 2400};//赤，緑，青
-
-int startP = 35;
-int downP = 5;
-
-int Asasult = 0;
-int Bsasult = 0;
-int Csasult = 0;
-int Dsasult = 0;
-
-bool compA = false;
-bool compB = false;
-bool compC = false;
-bool compD = false;
-
-bool invationA = false;
-bool invationB = false;
-bool invationC = false;
-bool invationD = false;
-
-Ticker Color_T;
-
-void ColorDetection();
-void Color_changeflag();
-
-//************ライントレース変数*******************
-
-//************ROタコン******************
-QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
-QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
-Ticker get_rpm;
-PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0);
-PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
-double rpmX;
-double rpmY;
-double disX;
-double disY;
-int palseX;
-int palseY;
-int RtpwmX;
-int RtpwmY;
-double goalX = 900.000;
-double goalY = 700.000;
-
-//double goalXB = 900.000;
-//double goalYB = 700.000;
-void filip();
-void filipB();
-
-//************ROタコン******************
-
-//************ジャイロ*******************
-bool Angle_flagI = false;
-int Angle;
-PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
-float rotateY;
-//初期値 -5
-int AngletargetX = 4;
-int AngletargetY = -12;
-int AngletargetI = -5;
-//************ジャイロ*******************
-
-//************Buzzer******************
+//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
-PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
-bool Emsflag = false;
-//************Buzzer******************
+bool EMGflag = false;
+PwmOut buzzer(BUZZER_PIN);
+//buzzer.period(1.0/800);
+//**************Buzzer****************
//************TapeLed*****************
void TapeLedEms_func();
@@ -179,6 +62,7 @@
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************
+
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -217,10 +101,7 @@
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;
@@ -303,11 +184,9 @@
while(1)
{
- getcolor();
- pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
- pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
- pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
- pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
+ //LED_DEBUG0 = !LED_DEBUG0;
+ //LED_DEBUG1 = !LED_DEBUG1;
+ //printf("%d\r\n",ECD_0.getPulses());
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
@@ -319,7 +198,7 @@
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
- buzzer = 0.5;
+ buzzer = 1;
BuzzerTimer.attach(BuzzerTimer_func, 0.5);
}
else
@@ -338,34 +217,17 @@
}
}
- if ((EMS_0 || EMS_1) && !Emsflag){
- buzzer = 0.5;
+ //Emergency!
+ if(EMG_0 || EMG_1){
+ buzzer = 1;
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) {
+ //Safety
+ if(!EMG_0 && !EMG_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();
@@ -380,338 +242,28 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
- tapeLED.code = (uint32_t)Green;
- if(RedSW){
- current = 1;
- }
- if(BlueSW){
- current = 2;
- }
+
}
#endif
#if USE_PROCESS_NUM>1
-static void Process1() //手動
+static void Process1()
{
- tapeLED.code = (uint32_t)Orange;
- motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
- motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
- motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
- motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
-
- motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
- motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
- motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
- motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
-
- if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
- motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
- motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
- }
- if(controller->Button.R){
- motor[Angle_R].dir = FOR;
- motor[Angle_L].dir = BACK;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
- }else if(controller->Button.L){
- motor[Angle_R].dir = BACK;
- motor[Angle_L].dir = FOR;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
- }else{
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
-
- if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
- /*for(int i = 0;i<20;i++){
- float y = 0;
- y = acc[1]*1000;
- float rotateY = (y - 305)/2.21 - 90;
- Angle += rotateY;
- }
- Angle = Angle/20;
- pc.printf("Y:%d \r\n",Angle);*/
-
- //wheel.getPulses()...どちらの方向にどれだけ回ったか
- //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
- //軸が何回転したか
- //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
}
#endif
#if USE_PROCESS_NUM>2
-static void Process2() //trace
+static void Process2()
{
- tapeLED.code = (uint32_t)Yellow;
- static bool color_flag = false;
-
- static bool traceon = false;//fase1
- static bool yokofla = false;//fase2
- static bool boxslip = false;//fase3
-
- //static bool syu = false;
-
- ColorDetection();
- Color_changeflag();
-
- if(controller->Button.B && !color_flag)
- {
- traceon = true;
- color_flag = true;
- }
- else if(!controller->Button.B)color_flag = false;
-
- if(traceon)
- {
- Color_changeflag();
- if(!invationA && !compA && !invationB && !compB)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(invationC && compC && !invationB && !compB)
- {
- for(int i = 0; i<1000; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- yokofla = true;
- traceon = false;
- }
- }
-
- if(yokofla && !traceon)
- {
- //pointcalculation();
- Color_changeflag();
- if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- wait(2);
-
- boxslip = true;
- yokofla = false;
- }
- else if(compA && compB && compC)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(invationA && invationB && invationC)
- {
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FREE;
-
- //motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- //motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && !invationB && !invationC)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- //motor[TIRE_FL].pwm = startP;
- //motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && compC && invationC)//C固定A下
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = 55;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(compA && compB && !invationC)//AB固定C下
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 55;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(compA && compB && !compC && invationC)//AB固定C上
- {
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = 55;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(!compA && invationA && compC)//C固定A上
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FOR;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = 55;
-
- Color_changeflag();
- }
- }
-
- if(boxslip)
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
}
#endif
#if USE_PROCESS_NUM>3
-static void Process3() //Blue Zone
+static void Process3()
{
- filipB();
-
- static bool Rt_flagX = false;
- static bool Rt_flagY = false;
- if(Rt_flagX)
- {
- filipB();
- if(disX < goalX - 5){
- filipB();
-
- 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)*0.8;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(disX > goalX - 5){
-
- for(int i = 0; i<200; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- Rt_flagY = true;
- Rt_flagX = false;
- }
- }
-
-if(Rt_flagY && !Rt_flagX){
- filipB();
- if(disY < goalY - 5){
- filipB();
- 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);
- }
- else if(disY > goalY - 5)
- {
- filipB();
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_FR].pwm = 255*0.85;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
- }
}
#endif
@@ -723,491 +275,27 @@
#endif
#if USE_PROCESS_NUM>5
-static void Process5() //ロタコンXY
-{
- tapeLED.code = (uint32_t)White;
-
- static bool nopushed = false;
- static bool Rt_flagX = false;
- static bool Rt_flagY = false;
+static void Process5()
+{
- if(controller->Button.A && !nopushed){
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }else if(!controller->Button.A)nopushed = false;
-
- filip();
-
- if(Rt_flagX)
- {
- filip();
- if(disX < goalX - 5){
- filip();
-
- 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)*0.8;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(disX > goalX - 5){
-
- for(int i = 0; i<200; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- Rt_flagY = true;
- Rt_flagX = false;
- }
- }
-
-
-if(Rt_flagY && !Rt_flagX){
- filip();
- if(disY < goalY - 5){
- filip();
- 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);
- }
- else if(disY > goalY - 5)
- {
- filip();
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_FR].pwm = 255*0.85;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
- }
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
- tapeLED.code = (uint32_t)Yellow;
- static bool color_flag = false;
- static bool traceon = false;//fase1
- static bool yokofla = false;//fase2
- static bool boxslip = false;//fase3
-
- static bool nopushed = false;
- static bool Rt_flagX = false;
- static bool Rt_flagY = false;
-
- //static bool syu = false;
-
- ColorDetection();
- Color_changeflag();
-
- if(controller->Button.B && !color_flag)
- {
- traceon = true;
- color_flag = true;
- }
- else if(!controller->Button.B)color_flag = false;
-
- if(traceon)
- {
- Color_changeflag();
- if(!invationA && !compA && !invationB && !compB)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(invationC && compC && !invationB && !compB)
- {
- for(int i = 0; i<1000; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- yokofla = true;
- traceon = false;
- }
- }
-
-
- if(yokofla && !traceon)
- {
- //pointcalculation();
- Color_changeflag();
- if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- wait(2);
-
- boxslip = true;
- yokofla = false;
- }
- else if(compA && compB && compC)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(invationA && invationB && invationC)
- {
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FREE;
-
- //motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- //motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && !invationB && !invationC)
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP;
- //motor[TIRE_FL].pwm = startP;
- //motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && compC && invationC)//C固定A下
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = 55;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
- }
- else if(compA && compB && !invationC)//AB固定C下
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 55;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(compA && compB && !compC && invationC)//AB固定C上
- {
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = 55;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(!compA && invationA && compC)//C固定A上
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FOR;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = 55;
-
- Color_changeflag();
- }
- }
-
- if(boxslip)
- {
- for(int i = 0; i<500; i++)
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }
-
-
- /*if(controller->Button.A && !nopushed){
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }else if(!controller->Button.A)nopushed = false;
- */
- filip();
-
- if(Rt_flagX)
- {
- filip();
- if(disX < goalX - 5){
- filip();
-
- 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)*0.8;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(disX > goalX - 5){
-
- for(int i = 0; i<500; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- Rt_flagY = true;
- Rt_flagX = false;
- }
- }
-
-
-if(Rt_flagY && !Rt_flagX){
- filip();
- if(disY < goalY - 5){
- filip();
- 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);
- }
- else if(disY > goalY - 5)
- {
- filip();
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
- }
-
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
- tapeLED.code = (uint32_t)Hotpink;
- static bool Xnopush = false;
- static bool Ynopush = false;
- static bool Inopush = false;
- static bool Angle_flagX = false;
- static bool Angle_flagY = false;
- static bool ANgle_flagI = false;
-
- if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
- for(int i = 0;i<20;i++){
- float y = 0;
- y = acc[1]*1000;
- float rotateY = (y - 305)/2.21 - 90;
- Angle += rotateY;
- }
- Angle = Angle /20;
-
- int gyropwmX = gyro.SetPV(Angle,AngletargetX);
- int gyropwmY = gyro.SetPV(Angle,AngletargetY);
- int gyropwmI = gyro.SetPV(Angle,AngletargetI);
-
- if(controller->Button.X && !Xnopush){
- Angle_flagX = true;
- Xnopush = true;
- }else if(!controller->Button.X)Xnopush = false;
-
- if(controller->Button.Y && !Ynopush){
- Angle_flagY = true;
- Ynopush = true;
- }else if(!controller->Button.Y)Ynopush = false;
-
- if(controller->Button.A && !Inopush){
- Angle_flagI = true;
- Inopush = true;
- }else if(!controller->Button.A)Inopush = false;
-
- if (Angle_flagX){
- motor[Angle_R].dir = SetStatus(gyropwmX);
- motor[Angle_L].dir = SetStatus(-gyropwmX);
- motor[Angle_R].pwm = SetPWM(gyropwmX);
- motor[Angle_L].pwm = SetPWM(gyropwmX);
- if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagX = false;
- }
- }
-
- if (Angle_flagY){
- motor[Angle_R].dir = SetStatus(-gyropwmY);
- motor[Angle_L].dir = SetStatus(gyropwmY);
- motor[Angle_R].pwm = SetPWM(gyropwmY);
- motor[Angle_L].pwm = SetPWM(gyropwmY);
- if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagY = false;
- }
- }
-
- if (Angle_flagI){
- if(Angle < 0)
- {
- motor[Angle_R].dir = SetStatus(-gyropwmI);
- motor[Angle_L].dir = SetStatus(gyropwmI);
- motor[Angle_R].pwm = SetPWM(gyropwmI);
- motor[Angle_L].pwm = SetPWM(gyropwmI);
-
- if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagI = false;
- }
- }
- else if(Angle > 0)
- {
- motor[Angle_R].dir = FOR;
- motor[Angle_L].dir = BACK;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
-
- if(Angle < 0){
- motor[Angle_R].dir = SetStatus(gyropwmI);
- motor[Angle_L].dir = SetStatus(-gyropwmI);
- motor[Angle_R].pwm = SetPWM(gyropwmI);
- motor[Angle_L].pwm = SetPWM(gyropwmI);
-
- if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagI = false;
- }
- }
- }
- }
- else{
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
}
#endif
-#if USE_PROCESS_NUM>8 //kakudo
+#if USE_PROCESS_NUM>8
static void Process8()
{
@@ -1239,169 +327,15 @@
#endif
}
-#pragma region USER-DEFINED-FUNCTIONS
-
-void filip(){
- palseX = RtX.getPulses();
- palseY = RtY.getPulses();
-
- rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
- rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
-
- disX = 48*3.141*rpmX;
- disY = 48*3.141*rpmY;
-
- RtpwmX = (int)Rt_X.SetPV(disX , goalX);
- RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
-}
-
-void filipB(){
- palseX = RtX.getPulses();
- palseY = RtY.getPulses();
-
- rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
- rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
-
- disX = abs(48*3.141*rpmX);
- disY = 48*3.141*rpmY;
-
- RtpwmX = (int)Rt_X.SetPV(disX , goalX);
- RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
-}
-
-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(){
- GATE = 0;
-
- CK[0] = 0;
- CK[1] = 0;
- CK[2] = 0;
- CK[3] = 0;
-
- RANGE = 1;
-
- GATE = 1;
- wait_ms(intergration);
- GATE = 0;
- wait_us(4);
-
- Color_A[0] = ColorIn(0); //赤
- wait_us(3);
- Color_A[1] = ColorIn(0); //青
- wait_us(3);
- Color_A[2] = ColorIn(0); //緑
-
- Color_B[0] = ColorIn(1);
- wait_us(3);
- Color_B[1] = ColorIn(1);
- wait_us(3);
- Color_B[2] = ColorIn(1);
-
- Color_C[0] = ColorIn(2);
- wait_us(3);
- Color_C[1] = ColorIn(2);
- wait_us(3);
- Color_C[2] = ColorIn(2);
-
- Color_D[0] = ColorIn(3);
- wait_us(3);
- Color_D[1] = ColorIn(3);
- wait_us(3);
- Color_D[2] = ColorIn(3);
-}
-
-void Color_changeflag(){
- ColorDetection();
-
- if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
- {
- invationA ^= 1;//start false,over true
- compA = true;//on true,noon false
- }
- else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶
-
- if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
- {
- invationB ^= 1;//start false,over true
- compB = true;//on true,noon false
- }
- else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶
-
- if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
- {
- invationC ^= 1;//start false,over true
- compC = true;//on true,noon false
- }
- else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶
- /*
- if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
- {
- invationD ^= 1;//start false,over true
- compD = true;//on true,noon false
- }
- else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
- */
-}
-
-void getcolor(){
- for(int i=0;i<10;i++){
- ColorDetection();
-
- Avecolor_A[0] += Color_A[0];
- Avecolor_A[1] += Color_A[1];
- Avecolor_A[2] += Color_A[2];
- Avecolor_B[0] += Color_B[0];
- Avecolor_B[1] += Color_B[1];
- Avecolor_B[2] += Color_B[2];
- Avecolor_C[0] += Color_C[0];
- Avecolor_C[1] += Color_C[1];
- Avecolor_C[2] += Color_C[2];
- Avecolor_D[0] += Color_D[0];
- Avecolor_D[1] += Color_D[1];
- Avecolor_D[2] += Color_D[2];
-}
-
-Avecolor_A[0] = Avecolor_A[0]/10;
-Avecolor_A[1] = Avecolor_A[1]/10;
-Avecolor_A[2] = Avecolor_A[2]/10;
-Avecolor_B[0] = Avecolor_B[0]/10;
-Avecolor_B[1] = Avecolor_B[1]/10;
-Avecolor_B[2] = Avecolor_B[2]/10;
-Avecolor_C[0] = Avecolor_C[0]/10;
-Avecolor_C[1] = Avecolor_C[1]/10;
-Avecolor_C[2] = Avecolor_C[2]/10;
-Avecolor_D[0] = Avecolor_D[0]/10;
-Avecolor_D[1] = Avecolor_D[1]/10;
-Avecolor_D[2] = Avecolor_D[2]/10;
-
-}
-
-void BuzzerTimer_func() {
- if(buzzer == 0.5){
- buzzer = 0;
- }
- else if(buzzer == 0){
- buzzer = 0.5;
- }
+void BuzzerTimer_func(){
+ buzzer = !buzzer;
}
void TapeLedEms_func() {
- sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
+ sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
+
+#pragma region USER-DEFINED-FUNCTIONS
+
+
#pragma endregion
diff -r dfcec98f5aa9 -r 3f2c2d89372b System/Process/Process.h
--- a/System/Process/Process.h Sun Oct 21 02:14:15 2018 +0000
+++ b/System/Process/Process.h Mon Jul 01 13:00:20 2019 +0000
@@ -3,31 +3,11 @@
#include "mbed.h"
-void SystemProcess();
-/*
-#define ROLLER_LF motor[ROLLER_LF_NUM]
-#define ROLLER_LL motor[ROLLER_LL_NUM]
-*/
-
-#define BUZZER_PIN PA_15 //ブザー
+#define BUZZER_PIN PC_5
+#define EMG_0 LimitSw::IsPressed(0)
+#define EMG_1 LimitSw::IsPressed(1)
-#define TIRE_FR 0 //足回り前右
-#define TIRE_FL 1 //足回り前左
-#define TIRE_BR 2 //足回り後右
-#define TIRE_BL 3 //足回り後左
-#define Angle_R 4 //角度調節右
-#define Angle_L 5 //角度調節左
-
-#define Lim_AR 3 //角度調節右
-#define Lim_AL 4 //角度調節左
-#define Lim_R 0 //センター右
-#define Lim_L 1 //センター左
-#define EMS_0 LimitSw::IsPressed(8) //非常停止ブザー0
-#define EMS_1 LimitSw::IsPressed(9) //非常停止ブザー1
-#define RedSW LimitSw::IsPressed(7) //赤ゾーン用スイッチ
-#define BlueSW LimitSw::IsPressed(6) //青ゾーン用スイッチ
-
-#define ROTATE_PER_REVOLUTIONS 50 //ロタコン
+void SystemProcess();
typedef union
{