miki sumito / Mbed 2 deprecated four_wheel_omni

四輪オムニの足回りを動かすプログラム

Dependencies:   mbed

main.cpp@1:cb4a6d5c77c8, 22 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?

UserRevisionLine numberNew 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 }