2019/09/13ver
Dependencies: SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen
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
diff -r 000000000000 -r 3ad208cbea5f lib/Eigen.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/IRsensor.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/PID.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/QEI.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/R1370.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/S-ShapeModel/position_controller.cpp --- /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]; +} +
diff -r 000000000000 -r 3ad208cbea5f lib/S-ShapeModel/position_controller.h --- /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 +
diff -r 000000000000 -r 3ad208cbea5f lib/SerialMultiByte.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/ikarashiMDC.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/measuring_wheel/Eigen.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/measuring_wheel/measuring_wheel.cpp --- /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; +}
diff -r 000000000000 -r 3ad208cbea5f lib/measuring_wheel/measuring_wheel.h --- /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
diff -r 000000000000 -r 3ad208cbea5f lib/omni_wheel.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f main.cpp --- /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]); + } + + } +}
diff -r 000000000000 -r 3ad208cbea5f mbed-os.lib --- /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
diff -r 000000000000 -r 3ad208cbea5f pin_config.h --- /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