nakashima Kohei / Mbed 2 deprecated SHIPBOT_LATEST

2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください

Dependencies:   mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase

main.cpp@0:f80bb69e638e, 2021-10-13 (annotated)

Committer:
hamohamo
Date:
Wed Oct 13 08:47:48 2021 +0000
Revision:
0:f80bb69e638e
Child:
1:aa9cc4250220
ROBOKON BTEAM SHIPBOT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hamohamo 0:f80bb69e638e 1 #define USESCRP
hamohamo 0:f80bb69e638e 2 #include "core.hpp"
hamohamo 0:f80bb69e638e 3 #include "SDFileSystem.h"
hamohamo 0:f80bb69e638e 4 #include "BNO055.h"
hamohamo 0:f80bb69e638e 5 #include <cmath>
hamohamo 0:f80bb69e638e 6 #include <vector>
hamohamo 0:f80bb69e638e 7
hamohamo 0:f80bb69e638e 8 #include "mbed.h"
hamohamo 0:f80bb69e638e 9 #define POINTS 900
hamohamo 0:f80bb69e638e 10
hamohamo 0:f80bb69e638e 11
hamohamo 0:f80bb69e638e 12 /*--------------FUNCTION--------------*/
hamohamo 0:f80bb69e638e 13 void init();
hamohamo 0:f80bb69e638e 14 void route_read();
hamohamo 0:f80bb69e638e 15 void getPoint(Position *p,int n);
hamohamo 0:f80bb69e638e 16 void getVelocity(double *v,int n);
hamohamo 0:f80bb69e638e 17 bool incircle(Position pos,Position target,double error);
hamohamo 0:f80bb69e638e 18 void gyroA(double theta);
hamohamo 0:f80bb69e638e 19 void gyroB(double theta);
hamohamo 0:f80bb69e638e 20 bool checkMOT12(int rx_data, int &tx_data);
hamohamo 0:f80bb69e638e 21 bool checkMOT34(int rx_data, int &tx_data);
hamohamo 0:f80bb69e638e 22
hamohamo 0:f80bb69e638e 23
hamohamo 0:f80bb69e638e 24 /*--------------VARIABLE--------------*/
hamohamo 0:f80bb69e638e 25
hamohamo 0:f80bb69e638e 26 std::vector<double> t_info,x_info,y_info,v_info,theta_info;
hamohamo 0:f80bb69e638e 27 Position pos,NextP,subP;
hamohamo 0:f80bb69e638e 28 int n=1;
hamohamo 0:f80bb69e638e 29 double v=0.0;
hamohamo 0:f80bb69e638e 30 double theta = 0.0;
hamohamo 0:f80bb69e638e 31 double gyro = 0.0;
hamohamo 0:f80bb69e638e 32 bool gyro_1 = false,gyro_2 = false;
hamohamo 0:f80bb69e638e 33 long long g_pulse;
hamohamo 0:f80bb69e638e 34 bool MOT12 = false,MOT34 = false;
hamohamo 0:f80bb69e638e 35
hamohamo 0:f80bb69e638e 36 BNO055 bno(PB_3, PB_10);
hamohamo 0:f80bb69e638e 37
hamohamo 0:f80bb69e638e 38 SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
hamohamo 0:f80bb69e638e 39 ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
hamohamo 0:f80bb69e638e 40 Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
hamohamo 0:f80bb69e638e 41 Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
hamohamo 0:f80bb69e638e 42
hamohamo 0:f80bb69e638e 43 int main(){
hamohamo 0:f80bb69e638e 44
hamohamo 0:f80bb69e638e 45 /*--------------SETUP--------------*/
hamohamo 0:f80bb69e638e 46 scrp.addCMD(6,checkMOT12);
hamohamo 0:f80bb69e638e 47 scrp.addCMD(7,checkMOT34);
hamohamo 0:f80bb69e638e 48 AKASHIKOSEN.setCWID(1,2,3,4);
hamohamo 0:f80bb69e638e 49 AKASHIKOSEN.setSWID(5,6,7,8);
hamohamo 0:f80bb69e638e 50
hamohamo 0:f80bb69e638e 51 RBT.addENC(PC_4,PA_13,512,4,1);
hamohamo 0:f80bb69e638e 52 RBT.addENC(PA_14,PA_15,512,4,2);
hamohamo 0:f80bb69e638e 53 RBT.addENC(PC_2,PC_3,512,4,3);
hamohamo 0:f80bb69e638e 54 RBT.addENC(PC_10,PC_11,512,4,4);
hamohamo 0:f80bb69e638e 55 RBT.addENC(PA_7,PA_6,512,4,5);
hamohamo 0:f80bb69e638e 56 RBT.addENC(PA_9,PA_8,512,4,6);
hamohamo 0:f80bb69e638e 57 RBT.addENC(PC_1,PC_0,512,4,7);
hamohamo 0:f80bb69e638e 58 RBT.addENC(PC_5,PA_12,512,4,8);
hamohamo 0:f80bb69e638e 59
hamohamo 0:f80bb69e638e 60 RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5);
hamohamo 0:f80bb69e638e 61 RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
hamohamo 0:f80bb69e638e 62 RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
hamohamo 0:f80bb69e638e 63 RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
hamohamo 0:f80bb69e638e 64 RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
hamohamo 0:f80bb69e638e 65
hamohamo 0:f80bb69e638e 66 init();
hamohamo 0:f80bb69e638e 67
hamohamo 0:f80bb69e638e 68 route_read();
hamohamo 0:f80bb69e638e 69
hamohamo 0:f80bb69e638e 70 wait(1);
hamohamo 0:f80bb69e638e 71
hamohamo 0:f80bb69e638e 72
hamohamo 0:f80bb69e638e 73 bool setup = false;
hamohamo 0:f80bb69e638e 74 if(!setup){
hamohamo 0:f80bb69e638e 75 bno.reset();
hamohamo 0:f80bb69e638e 76 bno.setmode(OPERATION_MODE_IMUPLUS);
hamohamo 0:f80bb69e638e 77 setup = true;
hamohamo 0:f80bb69e638e 78 }
hamohamo 0:f80bb69e638e 79
hamohamo 0:f80bb69e638e 80 RBT.START();
hamohamo 0:f80bb69e638e 81 /*--------------LOOP--------------*/
hamohamo 0:f80bb69e638e 82 while(true){
hamohamo 0:f80bb69e638e 83 pos = RBT.getStatus();
hamohamo 0:f80bb69e638e 84
hamohamo 0:f80bb69e638e 85 bno.get_angles();
hamohamo 0:f80bb69e638e 86 gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
hamohamo 0:f80bb69e638e 87 gyroA(gyro);
hamohamo 0:f80bb69e638e 88 gyroB(gyro);
hamohamo 0:f80bb69e638e 89
hamohamo 0:f80bb69e638e 90 theta = 2.0*M_PI*(int)(g_pulse/4.0);
hamohamo 0:f80bb69e638e 91 if(g_pulse > 0.0) theta += gyro;
hamohamo 0:f80bb69e638e 92 else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
hamohamo 0:f80bb69e638e 93 else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
hamohamo 0:f80bb69e638e 94 else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
hamohamo 0:f80bb69e638e 95
hamohamo 0:f80bb69e638e 96 RBT.setPosition(pos.x,pos.y,theta);
hamohamo 0:f80bb69e638e 97
hamohamo 0:f80bb69e638e 98 //RBT.setVelocity(0.0,0.0,0.0);
hamohamo 0:f80bb69e638e 99 if(n<POINTS and
hamohamo 0:f80bb69e638e 100 ( (incircle(pos,NextP,30.0) and n < POINTS and n != 1000) or
hamohamo 0:f80bb69e638e 101 ( incircle(pos,NextP,10.0) and n == 1000 ) or
hamohamo 0:f80bb69e638e 102 ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS)
hamohamo 0:f80bb69e638e 103 )
hamohamo 0:f80bb69e638e 104 )
hamohamo 0:f80bb69e638e 105 {
hamohamo 0:f80bb69e638e 106 n++;
hamohamo 0:f80bb69e638e 107 getPoint(&NextP,n);
hamohamo 0:f80bb69e638e 108 getVelocity(&v,n-1);
hamohamo 0:f80bb69e638e 109 if(n < POINTS)getPoint(&subP,n+1);
hamohamo 0:f80bb69e638e 110 if(v > 0.0) v *= 1.7;
hamohamo 0:f80bb69e638e 111 if(v >= 1400) v = 1400;
hamohamo 0:f80bb69e638e 112 //v = 200.0;
hamohamo 0:f80bb69e638e 113 if(n == POINTS) v = 50.0;
hamohamo 0:f80bb69e638e 114 printf("%d:pass\n",n);
hamohamo 0:f80bb69e638e 115 }
hamohamo 0:f80bb69e638e 116 if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
hamohamo 0:f80bb69e638e 117 double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
hamohamo 0:f80bb69e638e 118 RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02);
hamohamo 0:f80bb69e638e 119 RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getGain());
hamohamo 0:f80bb69e638e 120 //RBT.sendVelocity(200.0,200.0,0.0);
hamohamo 0:f80bb69e638e 121 printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
hamohamo 0:f80bb69e638e 122 RBT.LOOP();
hamohamo 0:f80bb69e638e 123 }
hamohamo 0:f80bb69e638e 124 }
hamohamo 0:f80bb69e638e 125
hamohamo 0:f80bb69e638e 126 void route_read() {
hamohamo 0:f80bb69e638e 127 static double x, y, v, theta_;
hamohamo 0:f80bb69e638e 128 wait(1);
hamohamo 0:f80bb69e638e 129 printf("Hello World!\n");
hamohamo 0:f80bb69e638e 130 wait(0.1);
hamohamo 0:f80bb69e638e 131 FILE *fp;
hamohamo 0:f80bb69e638e 132 do {
hamohamo 0:f80bb69e638e 133 fp = fopen("/sd/route.txt", "r");
hamohamo 0:f80bb69e638e 134 wait(0.2);
hamohamo 0:f80bb69e638e 135 } while(fp==NULL);
hamohamo 0:f80bb69e638e 136 printf("Good!\n");
hamohamo 0:f80bb69e638e 137 while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) {
hamohamo 0:f80bb69e638e 138 printf("%lf %lf %lf %lf\n",x, y, v, theta_);
hamohamo 0:f80bb69e638e 139 x_info.push_back(x);
hamohamo 0:f80bb69e638e 140 y_info.push_back(y);
hamohamo 0:f80bb69e638e 141 v_info.push_back(v);
hamohamo 0:f80bb69e638e 142 theta_info.push_back(theta_);
hamohamo 0:f80bb69e638e 143 }
hamohamo 0:f80bb69e638e 144 printf("checking...\n");
hamohamo 0:f80bb69e638e 145 for(int k=0;k<x_info.size();k++){
hamohamo 0:f80bb69e638e 146 printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
hamohamo 0:f80bb69e638e 147 }
hamohamo 0:f80bb69e638e 148 fclose(fp);
hamohamo 0:f80bb69e638e 149 }
hamohamo 0:f80bb69e638e 150
hamohamo 0:f80bb69e638e 151 void getPoint(Position *p,int n){
hamohamo 0:f80bb69e638e 152 p->x = x_info[n];
hamohamo 0:f80bb69e638e 153 p->y = y_info[n];
hamohamo 0:f80bb69e638e 154 p->theta = theta_info[n];
hamohamo 0:f80bb69e638e 155 }
hamohamo 0:f80bb69e638e 156
hamohamo 0:f80bb69e638e 157 void getVelocity(double *v,int n){
hamohamo 0:f80bb69e638e 158 *v = v_info[n];
hamohamo 0:f80bb69e638e 159 }
hamohamo 0:f80bb69e638e 160
hamohamo 0:f80bb69e638e 161 bool incircle(Position pos,Position target,double error){
hamohamo 0:f80bb69e638e 162 return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error);
hamohamo 0:f80bb69e638e 163 }
hamohamo 0:f80bb69e638e 164 void gyroA(double theta){
hamohamo 0:f80bb69e638e 165 static bool prev = false;
hamohamo 0:f80bb69e638e 166 if(0 <= theta and theta < M_PI) gyro_1 = false;
hamohamo 0:f80bb69e638e 167 else gyro_1 = true;
hamohamo 0:f80bb69e638e 168 if(gyro_1 and !prev){
hamohamo 0:f80bb69e638e 169 if(gyro_2) g_pulse++;
hamohamo 0:f80bb69e638e 170 else if(g_pulse != 0) g_pulse--;
hamohamo 0:f80bb69e638e 171 }
hamohamo 0:f80bb69e638e 172 else if(!gyro_1 and prev){
hamohamo 0:f80bb69e638e 173 if(gyro_2) g_pulse--;
hamohamo 0:f80bb69e638e 174 else if(g_pulse != 0)g_pulse++;
hamohamo 0:f80bb69e638e 175 }
hamohamo 0:f80bb69e638e 176 prev = gyro_1;
hamohamo 0:f80bb69e638e 177 }
hamohamo 0:f80bb69e638e 178 void gyroB(double theta){
hamohamo 0:f80bb69e638e 179 static bool prev = false;
hamohamo 0:f80bb69e638e 180 if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false;
hamohamo 0:f80bb69e638e 181 else gyro_2 = true;
hamohamo 0:f80bb69e638e 182 if(gyro_2 and !prev){
hamohamo 0:f80bb69e638e 183 if(gyro_1) g_pulse--;
hamohamo 0:f80bb69e638e 184 else g_pulse++;
hamohamo 0:f80bb69e638e 185 }
hamohamo 0:f80bb69e638e 186 else if(!gyro_2 and prev){
hamohamo 0:f80bb69e638e 187 if(gyro_1) g_pulse++;
hamohamo 0:f80bb69e638e 188 else g_pulse--;
hamohamo 0:f80bb69e638e 189 }
hamohamo 0:f80bb69e638e 190 prev = gyro_2;
hamohamo 0:f80bb69e638e 191 }
hamohamo 0:f80bb69e638e 192
hamohamo 0:f80bb69e638e 193 bool checkMOT12(int rx_data, int &tx_data){
hamohamo 0:f80bb69e638e 194 MOT12 = true;
hamohamo 0:f80bb69e638e 195 return true;
hamohamo 0:f80bb69e638e 196 }
hamohamo 0:f80bb69e638e 197 bool checkMOT34(int rx_data, int &tx_data){
hamohamo 0:f80bb69e638e 198 MOT34 = true;
hamohamo 0:f80bb69e638e 199 return true;
hamohamo 0:f80bb69e638e 200 }
hamohamo 0:f80bb69e638e 201
hamohamo 0:f80bb69e638e 202 void init(){
hamohamo 0:f80bb69e638e 203 MOT12 = false,MOT34 = false;
hamohamo 0:f80bb69e638e 204 scrp.send1(255,5,1);
hamohamo 0:f80bb69e638e 205 while(!MOT12 or !MOT34);
hamohamo 0:f80bb69e638e 206 }