miki sumito
/
four_wheel_omni
四輪オムニの足回りを動かすプログラム
main.cpp@1:cb4a6d5c77c8, 24 months ago (annotated)
- Committer:
- m_smt
- Date:
- Sat Oct 08 11:14:39 2022 +0000
- Revision:
- 1:cb4a6d5c77c8
- Parent:
- 0:39c2bb18192b
move four wheel omni
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m_smt | 0:39c2bb18192b | 1 | #include <iostream> |
m_smt | 0:39c2bb18192b | 2 | using namespace std; |
m_smt | 0:39c2bb18192b | 3 | #include "mbed.h" |
m_smt | 0:39c2bb18192b | 4 | #include "BNO055.hpp" |
m_smt | 0:39c2bb18192b | 5 | #include "motor.hpp" |
m_smt | 0:39c2bb18192b | 6 | #include "PID.hpp" |
m_smt | 0:39c2bb18192b | 7 | #include "rotaryinc.hpp" |
m_smt | 0:39c2bb18192b | 8 | #include "SBDBT.hpp" |
m_smt | 0:39c2bb18192b | 9 | |
m_smt | 0:39c2bb18192b | 10 | #define kp 0.00005 |
m_smt | 0:39c2bb18192b | 11 | #define ki 0.00000001 |
m_smt | 0:39c2bb18192b | 12 | #define kd 0.00000001 |
m_smt | 0:39c2bb18192b | 13 | #define dt 0.05 |
m_smt | 0:39c2bb18192b | 14 | #define diameter 101.6 |
m_smt | 0:39c2bb18192b | 15 | #define resolution 256.0 |
m_smt | 0:39c2bb18192b | 16 | |
m_smt | 0:39c2bb18192b | 17 | int main() |
m_smt | 0:39c2bb18192b | 18 | { |
m_smt | 0:39c2bb18192b | 19 | BNO055 bno(D3,D6); |
m_smt | 0:39c2bb18192b | 20 | sbdbt sbd(A0,A1); //アナログが対応しているピンを設定するべき |
m_smt | 0:39c2bb18192b | 21 | bno.reset(); //BNO初期化 |
m_smt | 0:39c2bb18192b | 22 | bno.setmode(OPERATION_MODE_IMUPLUS); //角度を読み取れるようにするモード? |
m_smt | 0:39c2bb18192b | 23 | |
m_smt | 0:39c2bb18192b | 24 | Motor mot[4] = { |
m_smt | 0:39c2bb18192b | 25 | Motor(PC_8,PC_9), |
m_smt | 0:39c2bb18192b | 26 | Motor(PB_6,PB_7), |
m_smt | 0:39c2bb18192b | 27 | Motor(PB_4,PB_5), |
m_smt | 0:39c2bb18192b | 28 | Motor(PB_13,PB_14) |
m_smt | 0:39c2bb18192b | 29 | }; |
m_smt | 0:39c2bb18192b | 30 | |
m_smt | 0:39c2bb18192b | 31 | PID pid[4] = { |
m_smt | 0:39c2bb18192b | 32 | PID(kp,ki,kd,dt), |
m_smt | 0:39c2bb18192b | 33 | PID(kp,ki,kd,dt), |
m_smt | 0:39c2bb18192b | 34 | PID(kp,ki,kd,dt), |
m_smt | 0:39c2bb18192b | 35 | PID(kp,ki,kd,dt) |
m_smt | 0:39c2bb18192b | 36 | }; |
m_smt | 0:39c2bb18192b | 37 | |
m_smt | 0:39c2bb18192b | 38 | rotaryinc rot[4] = { |
m_smt | 0:39c2bb18192b | 39 | rotaryinc(PC_5,PA_12,diameter,resolution,dt), |
m_smt | 0:39c2bb18192b | 40 | rotaryinc(PC_1,PC_0,diameter,resolution,dt), |
m_smt | 0:39c2bb18192b | 41 | rotaryinc(PA_9,PA_8,diameter,resolution,dt), |
m_smt | 0:39c2bb18192b | 42 | rotaryinc(PA_7,PA_6,diameter,resolution,dt) |
m_smt | 0:39c2bb18192b | 43 | }; |
m_smt | 0:39c2bb18192b | 44 | |
m_smt | 0:39c2bb18192b | 45 | double Theta = 0.0; |
m_smt | 0:39c2bb18192b | 46 | double tar[4] = {0.0}; |
m_smt | 0:39c2bb18192b | 47 | double speed[4] = {0.0}; |
m_smt | 0:39c2bb18192b | 48 | double vx = 0.0; |
m_smt | 0:39c2bb18192b | 49 | double vy = 0.0; |
m_smt | 0:39c2bb18192b | 50 | double rotation = 0.0; |
m_smt | 0:39c2bb18192b | 51 | double vx_tar_limit = 1000.0; |
m_smt | 0:39c2bb18192b | 52 | double vy_tar_limit = 1000.0; |
m_smt | 0:39c2bb18192b | 53 | double rotation_tar_limit = 1000.0; |
m_smt | 0:39c2bb18192b | 54 | bool mode = 1; |
m_smt | 0:39c2bb18192b | 55 | |
m_smt | 0:39c2bb18192b | 56 | while(1) { |
m_smt | 0:39c2bb18192b | 57 | |
m_smt | 0:39c2bb18192b | 58 | bno.get_angles(); //角度を取得 |
m_smt | 0:39c2bb18192b | 59 | Theta = bno.euler.yaw * (M_PI / 180); //弧度法に直す |
m_smt | 0:39c2bb18192b | 60 | |
m_smt | 0:39c2bb18192b | 61 | for(int i = 0; i < 4; i++) { |
m_smt | 0:39c2bb18192b | 62 | speed[i] = rot[i].getspeed(); //モータのmm/sをrotaryincのクラスから取得 |
m_smt | 0:39c2bb18192b | 63 | } |
m_smt | 0:39c2bb18192b | 64 | |
m_smt | 0:39c2bb18192b | 65 | vx = (vx_tar_limit / 64) * (sbd.lsx() - 64); //X方向の最大目標値決め |
m_smt | 0:39c2bb18192b | 66 | vy = (vy_tar_limit / 64) * (sbd.lsy() - 64); //Y方向の最大目標値決め |
m_smt | 0:39c2bb18192b | 67 | rotation = (rotation_tar_limit / 255) * (sbd.r2An() - sbd.l2An()); //回転運動の最大目標値決め |
m_smt | 0:39c2bb18192b | 68 | |
m_smt | 0:39c2bb18192b | 69 | |
m_smt | 0:39c2bb18192b | 70 | |
m_smt | 0:39c2bb18192b | 71 | //目標値決め |
m_smt | 0:39c2bb18192b | 72 | if(mode){ |
m_smt | 0:39c2bb18192b | 73 | tar[0] = vx * cos(M_PI/4 + Theta) + vy * sin(M_PI/4 + Theta) + rotation; |
m_smt | 0:39c2bb18192b | 74 | tar[1] = -vx * sin(M_PI/4 + Theta) + vy * cos(M_PI/4 + Theta) + rotation; |
m_smt | 0:39c2bb18192b | 75 | tar[2] = -vx * cos(M_PI/4 + Theta) - vy * sin(M_PI/4 + Theta) + rotation; |
m_smt | 0:39c2bb18192b | 76 | tar[3] = vx * sin(M_PI/4 + Theta) - vy * cos(M_PI/4 + Theta) + rotation; |
m_smt | 0:39c2bb18192b | 77 | } |
m_smt | 0:39c2bb18192b | 78 | |
m_smt | 0:39c2bb18192b | 79 | else { |
m_smt | 0:39c2bb18192b | 80 | tar[0] = vx * cos(M_PI/4) + vy * sin(M_PI/4) + rotation; |
m_smt | 0:39c2bb18192b | 81 | tar[1] = -vx * sin(M_PI/4) + vy * cos(M_PI/4) + rotation; |
m_smt | 0:39c2bb18192b | 82 | tar[2] = -vx * cos(M_PI/4) - vy * sin(M_PI/4) + rotation; |
m_smt | 0:39c2bb18192b | 83 | tar[3] = vx * sin(M_PI/4) - vy * cos(M_PI/4) + rotation; |
m_smt | 0:39c2bb18192b | 84 | } |
m_smt | 0:39c2bb18192b | 85 | |
m_smt | 0:39c2bb18192b | 86 | for(int i = 0; i < 4; i++) { |
m_smt | 0:39c2bb18192b | 87 | pid[i].set(speed[i],tar[i]); //現在値と目標値を引数として送ってクラス内でPID制御がされる |
m_smt | 0:39c2bb18192b | 88 | pid[i].safety(0.5,-0.5); //pwmの値が大きくなりすぎないようにpwmの最大値を設定しておく |
m_smt | 0:39c2bb18192b | 89 | mot[i].revolution(pid[i].pwm); //PID制御をしてでてきたpwmの値を引数として送ってモータが回る |
m_smt | 0:39c2bb18192b | 90 | } |
m_smt | 0:39c2bb18192b | 91 | |
m_smt | 0:39c2bb18192b | 92 | cout << "tar = " << tar[0] << " speed = " << speed[0] << endl; |
m_smt | 0:39c2bb18192b | 93 | rot[0].loop(); //dt秒ごとに動くプログラムになる |
m_smt | 0:39c2bb18192b | 94 | |
m_smt | 0:39c2bb18192b | 95 | } |
m_smt | 0:39c2bb18192b | 96 | } |