Revision 0:3ad208cbea5f, committed 2019-09-13

Comitter:

skouki

Date:

Fri Sep 13 02:19:03 2019 +0000

Commit message:

2019/09/13ver

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/Eigen.lib	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/IRsensor.lib	Fri Sep 13 02:19:03 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:19:03 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:19:03 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:19:03 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:19:03 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:19:03 2019 +0000
@@ -0,0 +1,38 @@
+#ifndef POSITION_CONTROLLER_H
+#define POSITION_CONTROLLER_H
+
+#include "mbed.h"
+#include "Geometry.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:19:03 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:19:03 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/measuring_wheel/Eigen.lib	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/measuring_wheel/measuring_wheel.cpp	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,123 @@
+#include "measuring_wheel.h"
+
+MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX):
+    w1(channel1_1, channel1_2, NC, PPR, QEI::X4_ENCODING),
+    w2(channel2_1, channel2_2, NC, PPR, QEI::X4_ENCODING),
+    w3(channel3_1, channel3_2, NC, PPR, QEI::X4_ENCODING),
+    r1370(R1370TX, R1370RX)
+{
+    miniX = 0;
+    miniY = 0;
+    X = 0;
+    Y = 0;
+    loopCounter = 0;
+
+    /*けいそくりんの設定*/
+    //ホイールの向き
+    radian[0] = -1 * PIII / 2.0;
+    radian[1] = PIII / 6.0;
+    radian[2] = 5.0 * PIII / 6.0;
+    //直径
+    diameter[0] = 49.0;
+    diameter[1] = 49.0;
+    diameter[2] = 49.0;
+    //中心からの距離
+    distance[0] = 124.2;
+    distance[1] = 124.2;
+    distance[2] = 124.2;
+
+    coefficient(0) = cos(radian[0]);
+    coefficient(1) = cos(radian[1]);
+    coefficient(2) = cos(radian[2]);
+    coefficient(3) = sin(radian[0]);
+    coefficient(4) = sin(radian[1]);
+    coefficient(5) = sin(radian[2]);
+    coefficient(6) = distance[0];
+    coefficient(7) = distance[1];
+    coefficient(8) = distance[2];
+
+    dec = coefficient.fullPivHouseholderQr();
+
+    thread.start(callback(this, &MeasuringWheel::threadloop));
+}
+
+void MeasuringWheel::computeXY()
+{
+    subY = 0;
+    subX = 0;
+    wheel(0) =  -diameter[0]*PIII/(PPR*4.0)*w1.getPulses();
+    wheel(1) =  -diameter[1]*PIII/(PPR*4.0)*w2.getPulses();
+    wheel(2) =  -diameter[2]*PIII/(PPR*4.0)*w3.getPulses();
+    w1.reset();
+    w2.reset();
+    w3.reset();
+    yawdegree = r1370.getAngle();
+    yaw = -1.0*yawdegree;
+    yaw *= PIII/180.0;
+
+    ans = dec.solve(wheel);
+
+    subX = ans(0);
+    subY = ans(1);
+
+    loopCounter++;
+    if(loopCounter > 1000000){
+        loopCounter = 0;
+        X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
+        Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
+        miniX = 0;
+        miniY = 0;
+    }else{
+        miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
+        miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
+    }
+
+    minusX = (X + miniX) * -1;
+    minusY = (Y + miniY) * -1;
+}
+
+void MeasuringWheel::threadloop()
+{
+    while(true) {
+        computeXY();
+    }
+}
+
+double MeasuringWheel::getjyroAngle()
+{
+    return r1370.getAngle();
+    //return phi;
+}
+
+double MeasuringWheel::getOutX()
+{
+    return minusX;
+}
+
+double MeasuringWheel::getOutY()
+{
+    return -1*minusY;
+}
+
+double MeasuringWheel::getWheel1()
+{
+    return w1.getPulses();
+}
+
+double MeasuringWheel::getWheel2()
+{
+    return w2.getPulses();
+}
+
+double MeasuringWheel::getWheel3()
+{
+    return w3.getPulses();
+}
+
+
+
+void MeasuringWheel::resetposition()
+{
+    X = 0;
+    Y = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/measuring_wheel/measuring_wheel.h	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,47 @@
+#ifndef MEASURING_WHEEL_H
+#define MEASURING_WHEEL_H
+
+#include "mbed.h"
+#include "QEI.h"
+#include "R1370.h"
+#include "Dense.h"
+
+#define FIRST_HEDDER 0xEE
+#define SECOND_HEDDER 0xFF
+#define PPR 500.0
+#define PIII 3.14159265358979
+
+class MeasuringWheel
+{
+public :
+    MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX);
+
+    double getOutX();
+    double getOutY();
+    double getWheel1(),getWheel2(),getWheel3();
+    double getjyroAngle();
+    double yaw;
+private :
+    double diameter[3];
+    double distance[3];
+    double radian[3];
+    Eigen::Matrix3d coefficient;
+    Eigen::Vector3d wheel;
+    Eigen::FullPivHouseholderQR<Eigen::Matrix3d> dec;
+    Eigen::Vector3d ans;
+    double miniX,miniY,X,Y,subX,subY,minusX,minusY;
+    int loopCounter;
+    void computeXY();
+    void threadloop();
+
+    QEI w1;
+    QEI w2;
+    QEI w3;
+    R1370 r1370;
+    double Cdif, yawdegree;
+    Thread thread;
+    uint16_t X_, Y_,ofsetX, ofsetY, yawdegree_;
+    void resetposition();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/omni_wheel.lib	Fri Sep 13 02:19:03 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:19:03 2019 +0000
@@ -0,0 +1,285 @@
+#include"mbed.h"
+#include"measuring_wheel.h"
+#include"ikarashiMDC.h"
+#include"position_controller.h"
+#include"omni_wheel.h"
+#include"pin_config.h"
+#include"SerialMultiByte.h"
+#include"PID.h"
+#include"IRsensor.h"
+
+#define YPOINT 4000
+#define GAP 0
+
+Serial serial(MDCTX,MDCRX,115200);
+ikarashiMDC motor[]={
+    ikarashiMDC(1,0,SM,&serial),
+    ikarashiMDC(1,1,SM,&serial),
+    ikarashiMDC(1,2,SM,&serial),
+    ikarashiMDC(1,3,SM,&serial)
+};
+PositionController position_control_1(500,500,0.1,0.01,0.8);
+
+OmniWheel omni(4);
+SerialMultiByte s(SERIALTX,SERIALRX);
+MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
+PID pid_spin(0,0,0,0.001);
+PID pid_x(0,0,0,0.001);
+PID pid_y(0,0,0,0.001);
+Serial pc(USBTX,USBRX,115200);
+
+DigitalIn debug_button(USER_BUTTON);
+DigitalOut debug_led_0(LED_0);
+DigitalOut debug_led_1(LED_2);
+DigitalOut debug_led_2(LED_1);
+
+IRsensor IR0(IR_0);
+
+int mode;
+int instruction_mode;
+double omni_power[4];
+double X_power,Y_power;
+double spin_power;
+float y_point = YPOINT;
+int X_,Y_;
+double dai_x,dai_low_y;
+int gap = GAP;
+
+void set_up()
+{
+  float theta = PIII / 4;
+  omni.wheel[0].setRadian(PIII - theta);
+  omni.wheel[1].setRadian(theta);
+  omni.wheel[2].setRadian(-theta);
+  omni.wheel[3].setRadian(PIII + theta);
+
+  s.setHeaders('A','Z');
+  s.startReceive(5);
+
+  IR0.startAveraging(5);
+
+}
+
+void mode1()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  position_control_1.compute(0.0,m.getOutY());
+  Y_power += position_control_1.getVelocityY();
+
+  pid_y.setProcessValue(m.getOutY() - Y_);
+  Y_power += pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode2()
+{
+  X_power -= 0.3;
+
+  pid_x.setProcessValue(m.getOutX() - X_);
+  X_power += pid_x.compute();
+
+  pid_y.setProcessValue(m.getOutY());
+  Y_power += pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode3()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  Y_power -= 0.3;
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode4()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  pid_y.setProcessValue(m.getOutY());
+  Y_power += pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void to_main()
+{
+  unsigned char data[1];
+  unsigned char getdata[5];
+  data[0] = mode;
+  s.sendData(data,1);
+  s.getData(getdata);
+  instruction_mode=getdata[0];
+
+  if(getdata[2]>=(1<<7)){
+    getdata[2]-=(1<<7);
+    X_=-1*(getdata[1]+(getdata[2]<<8));
+  }
+  else X_= getdata[1]+(getdata[2]<<8);
+
+  if(getdata[4]>=(1<<7)){
+    getdata[4]-=(1<<7);
+    Y_=-1*(getdata[3]+(getdata[4]<<8));
+  }
+  else Y_= getdata[3]+(getdata[4]<<8);
+  if(instruction_mode!=0)debug_led_1 = !debug_led_1;
+}
+int main()
+{
+  set_up();
+  while(true){
+    debug_led_0 = !debug_led_0;
+    X_power = 0.0;
+    Y_power = 0.0;
+    spin_power = 0.0;
+
+    to_main();
+
+    if(instruction_mode == 1&&mode == 0){
+
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(-1000.0,1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(0.0);
+
+      position_control_1.targetXY(1,int(y_point));
+
+      pid_y.reset();
+      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setInputLimits(-1000.0,1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(0.0);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 1;
+    }
+
+    if((m.getOutY() >= (y_point - 50)) && mode == 1){
+
+      pid_x.reset();
+      pid_x.setTunings(1.0,1.0,0.000001);
+      pid_x.setInputLimits(-1000.0,1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(0.0);
+
+      pid_y.reset();
+      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(y_point);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 2;
+    }
+
+    if(IR0.get_Averagingdistance()<=20&&mode == 2){
+      dai_x = m.getOutX();
+      mode = 0xff;
+    }
+
+    if(instruction_mode == 3&&mode == 0xff){
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(dai_x);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 3;
+    }
+
+    if(IR0.get_Averagingdistance()>=50&&mode == 3){
+      dai_low_y = m.getOutY();
+
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(dai_x);
+
+      pid_y.reset();
+      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setInputLimits(dai_low_y + gap - 1000.0 ,dai_low_y + gap + 1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(dai_low_y + gap);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 4;
+    }
+
+    if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
+      mode = 0xff;
+    }
+
+    if(mode == 1)mode1();
+    if(mode == 2)mode2();
+    if(mode == 3)mode3();
+    if(mode == 4)mode4();
+
+    omni.computeXY(Y_power,-X_power,-spin_power);
+
+    for(int i = 0; i < 4; i++){
+      omni_power[i] = 0.0;
+      omni_power[i] = omni.wheel[i];
+      motor[i].setSpeed(omni_power[i]);
+    }
+
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#1bf6b20df9d3cd5f29f001ffc6f0d0fcbbb96118

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pin_config.h	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,26 @@
+#ifndef PIN_CONFIG_H
+#define PIN_CONFIG_H
+
+#define LED_0 PB_10
+#define LED_1 PA_8
+#define LED_2 PA_9
+#define IR_0 PC_1
+#define IR_1 PC_0
+#define QEI1_1 PC_8
+#define QEI1_2 PC_5
+#define QEI2_1 PA_12
+#define QEI2_2 PA_11
+#define QEI3_1 PB_12
+#define QEI3_2 PB_2
+#define QEI4_1 PB_1
+#define QEI4_2 PB_15
+#define SERIALTX PA_0
+#define SERIALRX PA_1
+#define SERIALTX_2 PC_12
+#define SERIALRX_2 PD_2
+#define R1370TX PB_6
+#define R1370RX PA_10
+#define MDCTX PC_6
+#define MDCRX PC_7
+
+#endif