Kouki Suzuki / Mbed OS NHK2019_main_v6

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.

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver

Files at this revision

API Documentation at this revision

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

lib/IRsensor.lib Show annotated file Show diff for this revision Revisions of this file
lib/PID.lib Show annotated file Show diff for this revision Revisions of this file
lib/QEI.lib Show annotated file Show diff for this revision Revisions of this file
lib/R1370.lib Show annotated file Show diff for this revision Revisions of this file
lib/S-ShapeModel/position_controller.cpp Show annotated file Show diff for this revision Revisions of this file
lib/S-ShapeModel/position_controller.h Show annotated file Show diff for this revision Revisions of this file
lib/SerialMultiByte.lib Show annotated file Show diff for this revision Revisions of this file
lib/ikarashiMDC.lib Show annotated file Show diff for this revision Revisions of this file
lib/omni_wheel.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-os.lib Show annotated file Show diff for this revision Revisions of this file
pin_config.h Show annotated file Show diff for this revision Revisions of this file 
--- /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
+