Kouki Suzuki
2019/09/13ver
Dependencies: SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver
Revision 0:f68b460de813, committed 2019-09-13
- Comitter:
- skouki
- Date:
- Fri Sep 13 02:17:39 2019 +0000
- Commit message:
- 2019/09/13ver
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/IRsensor.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/IRsensor/#790cd18896a8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/PID.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/QEI.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/R1370.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/R1370MeasuringWheel/#ee51008e03e2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/S-ShapeModel/position_controller.cpp Fri Sep 13 02:17:39 2019 +0000
@@ -0,0 +1,81 @@
+#include "position_controller.h"
+
+PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
+ accDistance(accDistance_),
+ decDistance(decDistance_),
+ initialVelocity(initialVelocity_),
+ terminalVelocity(terminalVelocity_),
+ maxVelocity(maxVelocity_)
+{
+}
+
+void PositionController::targetXY(int targetX_, int targetY_)
+{
+ targetX = targetX_;
+ targetY = targetY_;
+ target = hypot((double)targetX, (double)targetY);
+ radians = atan2((double)targetY, (double)targetX);
+ if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
+ enoughDistance = true;
+ } else {
+ enoughDistance = false;
+ maxVelocity = initialVelocity + (maxVelocity - initialVelocity) * (target/(accDistance+decDistance));
+ accTrue = accDistance * (target/(accDistance+decDistance));
+ decTrue = decDistance * (target/(accDistance+decDistance));
+ }
+}
+
+double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
+{
+ return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
+}
+
+double PositionController::getVelocity(int position)
+{
+ double vel = 0;
+ if(enoughDistance) {
+ if(position < 0) {
+ vel = initialVelocity;
+ } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
+ vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
+ } else if(position < target - (decDistance * fabs(maxVelocity - terminalVelocity))) {
+ vel = maxVelocity;
+ } else if(position < target) {
+ vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decDistance * fabs(maxVelocity - terminalVelocity), decDistance * fabs(maxVelocity - terminalVelocity));
+ } else if(position < 2 * target) {
+ vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
+ } else {
+ vel = -maxVelocity;
+ }
+ } else {
+ if(position < 0) {
+ vel = initialVelocity;
+ } else if(position < accTrue) {
+ vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accTrue);
+ } else if(position < target) {
+ vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decTrue, decTrue);
+ } else if(position < 2 * target) {
+ vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
+ } else {
+ vel = -maxVelocity;
+ }
+ }
+ return vel;
+}
+
+void PositionController::compute(int positionX, int positionY)
+{
+ velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
+ velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
+}
+
+double PositionController::getVelocityX()
+{
+ return velocity[0];
+}
+
+double PositionController::getVelocityY()
+{
+ return velocity[1];
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/S-ShapeModel/position_controller.h Fri Sep 13 02:17:39 2019 +0000
@@ -0,0 +1,37 @@
+#ifndef POSITION_CONTROLLER_H
+#define POSITION_CONTROLLER_H
+
+#include "mbed.h"
+#define M_PI 3.141592653589793238462643383219502884
+
+class PositionController {
+ public :
+ PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_);
+
+ void compute(int positionX, int positionY);
+ void targetXY(int targetX_, int targetY_);
+ double getVelocityX();
+ double getVelocityY();
+ private :
+
+ double generateSineWave(double x, double initV, double termV, double start, double length);
+
+ double accDistance;
+ double decDistance;
+ double accTrue;
+ double decTrue;
+ double initialVelocity;
+ double terminalVelocity;
+ float maxVelocity;
+ double target;
+ int targetX;
+ int targetY;
+ double radians;
+ double velocity[2];
+
+ bool enoughDistance;
+
+ double getVelocity(int position);
+};
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/SerialMultiByte.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SerialMultiByte/#ca2a6fdb24af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/ikarashiMDC.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC_2byte_ver/#97bb662f1e1f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/omni_wheel.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Sep 13 02:17:39 2019 +0000
@@ -0,0 +1,68 @@
+#include"mbed.h"
+#include"SerialMultiByte.h"
+#include"pin_config.h"
+
+SerialMultiByte sita_s(S_SERIALTX,S_SERIALRX);
+SerialMultiByte ue_s(U_SERIALTX,U_SERIALRX);
+DigitalIn start(USER_BUTTON);
+Serial pc(USBTX,USBRX,115200);
+
+DigitalOut debug_led_0(LED0);
+DigitalOut debug_led_1(LED1);
+DigitalOut debug_led_2(LED2);
+DigitalIn debug_button(PC_4);
+
+int mode=0;
+int u_mode=0,s_mode=0,m_mode=0;
+unsigned char itidata[4];
+int X_,Y_;
+int data_a;
+
+void to_sita(){
+ unsigned char data[5];
+ unsigned char getdata[1];
+ data[0] = mode;
+ for(int i=0;i<4;i++){
+ data[i+1] = itidata[i];
+ }
+ sita_s.sendData(data,5);
+ sita_s.getData(getdata);
+ s_mode = getdata[0];
+}
+
+void to_ue(){
+ unsigned char data[1];
+ unsigned char getdata[5];
+ data[0] = mode;
+ ue_s.sendData(data,1);
+ ue_s.getData(getdata);
+ u_mode=getdata[0];
+ for(int i=0;i<4;i++){
+ itidata[i] = getdata[i+1];
+ }
+}
+
+
+int main()
+{
+ sita_s.setHeaders('A','Z');
+ sita_s.startReceive(1);
+ ue_s.setHeaders('A','Z');
+ ue_s.startReceive(5);
+ debug_button.mode(PullDown);
+ while(1)
+ {
+ debug_led_0 = !debug_led_0;
+ to_ue();
+ to_sita();
+ if(!start)mode = 1;
+ if(mode==1&&s_mode ==2)mode = 2;
+ if(mode == 2&&u_mode == 0xff &&s_mode == 0xff)mode = 3;
+ if(mode == 3&&u_mode == 0xff &&s_mode == 0xff)mode = 5;
+
+
+ }
+
+
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#88fb9b162d93a10e0d97f151c91bf2faf69e1b9e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pin_config.h Fri Sep 13 02:17:39 2019 +0000 @@ -0,0 +1,31 @@ +#ifndef PIN_CONFIG_H +#define PIN_CONFIG_H + +#define S_SERIALTX PA_0 +#define S_SERIALRX PA_1 + +#define U_SERIALTX PC_12 +#define U_SERIALRX PD_2 + +#define IR_0 PC_1 +#define IR_1 PC_0 +#define IR_2 PC_2 + +#define LED0 PB_10 +#define LED1 PA_8 +#define LED2 PA_9 + +#define MDCTX PC_6 +#define MDCRX PC_7 + +#define QEI_1_1 PC_8 +#define QEI_1_2 PC_5 +#define QEI_2_1 PA_12 +#define QEI_2_2 PA_11 +#define QEI_3_1 PB_12 +#define QEI_3_2 PB_2 +#define QEI_4_1 PB_1 +#define QEI_4_2 PB_15 + +#endif +