main.cpp

00001 #include <iostream>
00002 using namespace std;
00003 #include "mbed.h"
00004 #include "BNO055.hpp"
00005 #include "motor.hpp"
00006 #include "PID.hpp"
00007 #include "rotaryinc.hpp"
00008 #include "SBDBT.hpp"
00009 
00010 #define kp 0.00005
00011 #define ki 0.00000001
00012 #define kd 0.00000001
00013 #define dt 0.05
00014 #define diameter 101.6
00015 #define resolution 256.0
00016 
00017 int main()
00018 {
00019     BNO055 bno(D3,D6);
00020     sbdbt sbd(A0,A1);                       //アナログが対応しているピンを設定するべき
00021     bno.reset();                            //BNO初期化
00022     bno.setmode(OPERATION_MODE_IMUPLUS);    //角度を読み取れるようにするモード？
00023 
00024     Motor mot[4] = {
00025         Motor(PC_8,PC_9),
00026         Motor(PB_6,PB_7),
00027         Motor(PB_4,PB_5),
00028         Motor(PB_13,PB_14)
00029     };
00030 
00031     PID pid[4] = {
00032         PID(kp,ki,kd,dt),
00033         PID(kp,ki,kd,dt),
00034         PID(kp,ki,kd,dt),
00035         PID(kp,ki,kd,dt)
00036     };
00037 
00038     rotaryinc rot[4] = {
00039         rotaryinc(PC_5,PA_12,diameter,resolution,dt),
00040         rotaryinc(PC_1,PC_0,diameter,resolution,dt),
00041         rotaryinc(PA_9,PA_8,diameter,resolution,dt),
00042         rotaryinc(PA_7,PA_6,diameter,resolution,dt)
00043     };
00044 
00045     double Theta = 0.0;
00046     double tar[4] = {0.0};
00047     double speed[4] = {0.0};
00048     double vx = 0.0;
00049     double vy = 0.0;
00050     double rotation = 0.0;
00051     double vx_tar_limit = 1000.0;
00052     double vy_tar_limit = 1000.0;
00053     double rotation_tar_limit = 1000.0;
00054     bool mode = 1;
00055 
00056     while(1) {
00057 
00058         bno.get_angles();                       //角度を取得
00059         Theta = bno.euler.yaw * (M_PI / 180);   //弧度法に直す
00060 
00061         for(int i = 0; i < 4; i++) {
00062             speed[i] = rot[i].getspeed(); //モータのmm/sをrotaryincのクラスから取得
00063         }
00064 
00065         vx = (vx_tar_limit / 64) * (sbd.lsx() - 64);                        //X方向の最大目標値決め
00066         vy = (vy_tar_limit / 64) * (sbd.lsy() - 64);                        //Y方向の最大目標値決め
00067         rotation = (rotation_tar_limit / 255) * (sbd.r2An() - sbd.l2An());  //回転運動の最大目標値決め
00068 
00069 
00070 
00071         //目標値決め
00072         if(mode){
00073             tar[0] =  vx * cos(M_PI/4 + Theta) + vy * sin(M_PI/4 + Theta) + rotation;
00074             tar[1] = -vx * sin(M_PI/4 + Theta) + vy * cos(M_PI/4 + Theta) + rotation;
00075             tar[2] = -vx * cos(M_PI/4 + Theta) - vy * sin(M_PI/4 + Theta) + rotation;
00076             tar[3] =  vx * sin(M_PI/4 + Theta) - vy * cos(M_PI/4 + Theta) + rotation;
00077         }
00078 
00079         else {
00080             tar[0] =  vx * cos(M_PI/4) + vy * sin(M_PI/4) + rotation;
00081             tar[1] = -vx * sin(M_PI/4) + vy * cos(M_PI/4) + rotation;
00082             tar[2] = -vx * cos(M_PI/4) - vy * sin(M_PI/4) + rotation;
00083             tar[3] =  vx * sin(M_PI/4) - vy * cos(M_PI/4) + rotation;
00084         }
00085 
00086         for(int i = 0; i < 4; i++) {
00087             pid[i].set(speed[i],tar[i]);    //現在値と目標値を引数として送ってクラス内でPID制御がされる
00088             pid[i].safety(0.5,-0.5);        //pwmの値が大きくなりすぎないようにpwmの最大値を設定しておく
00089             mot[i].revolution(pid[i].pwm);  //PID制御をしてでてきたpwmの値を引数として送ってモータが回る
00090         }
00091 
00092         cout << "tar = " << tar[0] << "  speed = " << speed[0] << endl;
00093         rot[0].loop(); //dt秒ごとに動くプログラムになる
00094 
00095     }
00096 }