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@4:0038da6cdc9a, 2021-10-24 (annotated)

Committer:
hamohamo
Date:
Sun Oct 24 04:48:27 2021 +0000
Revision:
4:0038da6cdc9a
Parent:
2:4e7ec0816a91
Child:
5:aee4416b0d4f
asd

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 4:0038da6cdc9a 7 #include <ros.h>
hamohamo 4:0038da6cdc9a 8 #include <std_msgs/Int16.h>
hamohamo 0:f80bb69e638e 9
hamohamo 0:f80bb69e638e 10 #include "mbed.h"
hamohamo 0:f80bb69e638e 11 #define POINTS 900
hamohamo 4:0038da6cdc9a 12 #define ISLAND 50
hamohamo 0:f80bb69e638e 13
hamohamo 0:f80bb69e638e 14
hamohamo 0:f80bb69e638e 15 /*--------------FUNCTION--------------*/
hamohamo 1:aa9cc4250220 16 bool init();
hamohamo 0:f80bb69e638e 17 void route_read();
hamohamo 4:0038da6cdc9a 18 void spinonce();
hamohamo 0:f80bb69e638e 19 void getPoint(Position *p,int n);
hamohamo 0:f80bb69e638e 20 void getVelocity(double *v,int n);
hamohamo 0:f80bb69e638e 21 bool incircle(Position pos,Position target,double error);
hamohamo 0:f80bb69e638e 22 void gyroA(double theta);
hamohamo 0:f80bb69e638e 23 void gyroB(double theta);
hamohamo 0:f80bb69e638e 24 bool checkMOT12(int rx_data, int &tx_data);
hamohamo 0:f80bb69e638e 25 bool checkMOT34(int rx_data, int &tx_data);
hamohamo 4:0038da6cdc9a 26 void getPixel(const std_msgs::Int16& pix);
hamohamo 0:f80bb69e638e 27
hamohamo 0:f80bb69e638e 28
hamohamo 0:f80bb69e638e 29 /*--------------VARIABLE--------------*/
hamohamo 0:f80bb69e638e 30
hamohamo 0:f80bb69e638e 31 std::vector<double> t_info,x_info,y_info,v_info,theta_info;
hamohamo 4:0038da6cdc9a 32 Position pos,vel,NextP,subP;
hamohamo 0:f80bb69e638e 33 int n=1;
hamohamo 0:f80bb69e638e 34 double v=0.0;
hamohamo 0:f80bb69e638e 35 double theta = 0.0;
hamohamo 0:f80bb69e638e 36 double gyro = 0.0;
hamohamo 4:0038da6cdc9a 37 int pixel = 0.0;
hamohamo 0:f80bb69e638e 38 bool gyro_1 = false,gyro_2 = false;
hamohamo 0:f80bb69e638e 39 long long g_pulse;
hamohamo 0:f80bb69e638e 40 bool MOT12 = false,MOT34 = false;
hamohamo 0:f80bb69e638e 41
hamohamo 0:f80bb69e638e 42 BNO055 bno(PB_3, PB_10);
hamohamo 0:f80bb69e638e 43
hamohamo 4:0038da6cdc9a 44 ros::NodeHandle nh;
hamohamo 4:0038da6cdc9a 45 Ticker rosspin;
hamohamo 4:0038da6cdc9a 46
hamohamo 0:f80bb69e638e 47 SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
hamohamo 0:f80bb69e638e 48 ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
hamohamo 0:f80bb69e638e 49 Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
hamohamo 0:f80bb69e638e 50 Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
hamohamo 0:f80bb69e638e 51
hamohamo 4:0038da6cdc9a 52 ros::Subscriber<std_msgs::Int16> sub("pixel", &getPixel);
hamohamo 4:0038da6cdc9a 53
hamohamo 0:f80bb69e638e 54 int main(){
hamohamo 1:aa9cc4250220 55
hamohamo 4:0038da6cdc9a 56 PwmOut ok(PC_6);
hamohamo 0:f80bb69e638e 57
hamohamo 0:f80bb69e638e 58 /*--------------SETUP--------------*/
hamohamo 4:0038da6cdc9a 59
hamohamo 4:0038da6cdc9a 60 nh.getHardware()->setBaud(115200);
hamohamo 4:0038da6cdc9a 61 nh.initNode();
hamohamo 4:0038da6cdc9a 62 nh.subscribe(sub);
hamohamo 4:0038da6cdc9a 63 rosspin.attach_us(&spinonce,100);
hamohamo 4:0038da6cdc9a 64
hamohamo 4:0038da6cdc9a 65 scrp.addCMD(7,checkMOT12);
hamohamo 4:0038da6cdc9a 66 scrp.addCMD(8,checkMOT34);
hamohamo 4:0038da6cdc9a 67 AKASHIKOSEN.setCWID(1,2,3,4);
hamohamo 4:0038da6cdc9a 68 AKASHIKOSEN.setSWID(5,6,7,8);
hamohamo 4:0038da6cdc9a 69
hamohamo 4:0038da6cdc9a 70 RBT.addENC(PC_4,PA_13,512,4,1);
hamohamo 4:0038da6cdc9a 71 RBT.addENC(PA_14,PA_15,512,4,2);
hamohamo 4:0038da6cdc9a 72 RBT.addENC(PC_2,PC_3,512,4,3);
hamohamo 4:0038da6cdc9a 73 RBT.addENC(PC_10,PC_11,512,4,4);
hamohamo 4:0038da6cdc9a 74 RBT.addENC(PA_7,PA_6,512,4,5);
hamohamo 4:0038da6cdc9a 75 RBT.addENC(PA_9,PA_8,512,4,6);
hamohamo 4:0038da6cdc9a 76 RBT.addENC(PC_1,PC_0,512,4,7);
hamohamo 4:0038da6cdc9a 77 RBT.addENC(PC_5,PA_12,512,4,8);
hamohamo 0:f80bb69e638e 78
hamohamo 4:0038da6cdc9a 79 RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 80 RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 81 RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 82 RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 83 RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
hamohamo 4:0038da6cdc9a 84 RBT.addPID(0.01,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
hamohamo 4:0038da6cdc9a 85 //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
hamohamo 4:0038da6cdc9a 86 /*
hamohamo 4:0038da6cdc9a 87 RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 88 RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 89 RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 90 RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5);
hamohamo 4:0038da6cdc9a 91 RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
hamohamo 4:0038da6cdc9a 92 */
hamohamo 4:0038da6cdc9a 93 MOT12 = false,MOT34 = false;
hamohamo 4:0038da6cdc9a 94
hamohamo 4:0038da6cdc9a 95 while(init());
hamohamo 4:0038da6cdc9a 96 printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
hamohamo 4:0038da6cdc9a 97 route_read();
hamohamo 4:0038da6cdc9a 98
hamohamo 4:0038da6cdc9a 99 wait(1);
hamohamo 4:0038da6cdc9a 100 ok = 1.0;
hamohamo 0:f80bb69e638e 101
hamohamo 4:0038da6cdc9a 102 bool setup = false;
hamohamo 4:0038da6cdc9a 103 if(!setup){
hamohamo 4:0038da6cdc9a 104 bno.reset();
hamohamo 4:0038da6cdc9a 105 bno.setmode(OPERATION_MODE_IMUPLUS);
hamohamo 4:0038da6cdc9a 106 setup = true;
hamohamo 0:f80bb69e638e 107 }
hamohamo 4:0038da6cdc9a 108
hamohamo 4:0038da6cdc9a 109 RBT.START();
hamohamo 4:0038da6cdc9a 110 RBT.setPosition(0.0,0.0,0.0);
hamohamo 4:0038da6cdc9a 111 /*--------------LOOP--------------*/
hamohamo 4:0038da6cdc9a 112 while(true){
hamohamo 4:0038da6cdc9a 113 pos = RBT.getStatus(true);
hamohamo 4:0038da6cdc9a 114 bno.get_angles();
hamohamo 4:0038da6cdc9a 115 gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
hamohamo 4:0038da6cdc9a 116 gyroA(gyro);
hamohamo 4:0038da6cdc9a 117 gyroB(gyro);
hamohamo 4:0038da6cdc9a 118
hamohamo 4:0038da6cdc9a 119 theta = 2.0*M_PI*(int)(g_pulse/4.0);
hamohamo 4:0038da6cdc9a 120 if(g_pulse > 0.0) theta += gyro;
hamohamo 4:0038da6cdc9a 121 else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
hamohamo 4:0038da6cdc9a 122 else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
hamohamo 4:0038da6cdc9a 123 else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
hamohamo 4:0038da6cdc9a 124
hamohamo 4:0038da6cdc9a 125 RBT.setPosition(pos.x,pos.y,theta);
hamohamo 4:0038da6cdc9a 126
hamohamo 4:0038da6cdc9a 127 //RBT.setVelocity(0.0,0.0,0.0);
hamohamo 4:0038da6cdc9a 128 if(n<POINTS and
hamohamo 4:0038da6cdc9a 129 ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000 and n != ISLAND) or
hamohamo 4:0038da6cdc9a 130 ( incircle(pos,NextP,10.0) and n == 1000 ) or
hamohamo 4:0038da6cdc9a 131 ( 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 4:0038da6cdc9a 132 )
hamohamo 4:0038da6cdc9a 133 )
hamohamo 4:0038da6cdc9a 134 {
hamohamo 4:0038da6cdc9a 135 n++;
hamohamo 4:0038da6cdc9a 136 getPoint(&NextP,n);
hamohamo 4:0038da6cdc9a 137 NextP.theta -= M_PI/2.0;
hamohamo 4:0038da6cdc9a 138 getVelocity(&v,n-1);
hamohamo 4:0038da6cdc9a 139 if(n < POINTS)getPoint(&subP,n+1);
hamohamo 4:0038da6cdc9a 140 if(v > 0.0) v *= 1.5;
hamohamo 4:0038da6cdc9a 141 if(v >= 1400) v = 1400;
hamohamo 4:0038da6cdc9a 142 //v = 20.0;
hamohamo 4:0038da6cdc9a 143 if(n == POINTS) v = 20.0;
hamohamo 4:0038da6cdc9a 144 printf("%d:pass\n",n);
hamohamo 4:0038da6cdc9a 145 }
hamohamo 4:0038da6cdc9a 146
hamohamo 4:0038da6cdc9a 147 double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
hamohamo 4:0038da6cdc9a 148 RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
hamohamo 4:0038da6cdc9a 149 vel.theta = RBT.PIDs[5]->getmv();
hamohamo 4:0038da6cdc9a 150
hamohamo 4:0038da6cdc9a 151 if(n == ISLAND and incircle(pos,NextP,10.0) and abs(pixel) > 10){
hamohamo 4:0038da6cdc9a 152 RBT.PIDs[0]->Update(pixel,0,0.02,false);
hamohamo 4:0038da6cdc9a 153 vel.theta = RBT.PIDs[0]->getmv();
hamohamo 4:0038da6cdc9a 154 }
hamohamo 4:0038da6cdc9a 155 else {RBT.sendPWM(1,9);}
hamohamo 4:0038da6cdc9a 156
hamohamo 4:0038da6cdc9a 157 if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
hamohamo 4:0038da6cdc9a 158 if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) vel.theta = 0.0;
hamohamo 4:0038da6cdc9a 159
hamohamo 4:0038da6cdc9a 160 vel.x = v*cos(etheta);
hamohamo 4:0038da6cdc9a 161 vel.y = v*sin(etheta);
hamohamo 4:0038da6cdc9a 162
hamohamo 4:0038da6cdc9a 163 RBT.sendVelocity(vel.x,vel.y,vel.theta);
hamohamo 4:0038da6cdc9a 164 printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
hamohamo 4:0038da6cdc9a 165 RBT.LOOP();
hamohamo 4:0038da6cdc9a 166 }
hamohamo 0:f80bb69e638e 167 }
hamohamo 0:f80bb69e638e 168
hamohamo 0:f80bb69e638e 169 void route_read() {
hamohamo 0:f80bb69e638e 170 static double x, y, v, theta_;
hamohamo 0:f80bb69e638e 171 wait(1);
hamohamo 0:f80bb69e638e 172 printf("Hello World!\n");
hamohamo 0:f80bb69e638e 173 wait(0.1);
hamohamo 0:f80bb69e638e 174 FILE *fp;
hamohamo 0:f80bb69e638e 175 do {
hamohamo 0:f80bb69e638e 176 fp = fopen("/sd/route.txt", "r");
hamohamo 0:f80bb69e638e 177 wait(0.2);
hamohamo 0:f80bb69e638e 178 } while(fp==NULL);
hamohamo 0:f80bb69e638e 179 printf("Good!\n");
hamohamo 0:f80bb69e638e 180 while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) {
hamohamo 0:f80bb69e638e 181 printf("%lf %lf %lf %lf\n",x, y, v, theta_);
hamohamo 1:aa9cc4250220 182 x_info.push_back(x*2.0);
hamohamo 1:aa9cc4250220 183 y_info.push_back(y*1.5);
hamohamo 0:f80bb69e638e 184 v_info.push_back(v);
hamohamo 0:f80bb69e638e 185 theta_info.push_back(theta_);
hamohamo 0:f80bb69e638e 186 }
hamohamo 0:f80bb69e638e 187 printf("checking...\n");
hamohamo 0:f80bb69e638e 188 for(int k=0;k<x_info.size();k++){
hamohamo 0:f80bb69e638e 189 printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
hamohamo 0:f80bb69e638e 190 }
hamohamo 0:f80bb69e638e 191 fclose(fp);
hamohamo 0:f80bb69e638e 192 }
hamohamo 0:f80bb69e638e 193
hamohamo 0:f80bb69e638e 194 void getPoint(Position *p,int n){
hamohamo 0:f80bb69e638e 195 p->x = x_info[n];
hamohamo 0:f80bb69e638e 196 p->y = y_info[n];
hamohamo 0:f80bb69e638e 197 p->theta = theta_info[n];
hamohamo 0:f80bb69e638e 198 }
hamohamo 0:f80bb69e638e 199
hamohamo 0:f80bb69e638e 200 void getVelocity(double *v,int n){
hamohamo 0:f80bb69e638e 201 *v = v_info[n];
hamohamo 0:f80bb69e638e 202 }
hamohamo 0:f80bb69e638e 203
hamohamo 0:f80bb69e638e 204 bool incircle(Position pos,Position target,double error){
hamohamo 0:f80bb69e638e 205 return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error);
hamohamo 0:f80bb69e638e 206 }
hamohamo 0:f80bb69e638e 207 void gyroA(double theta){
hamohamo 0:f80bb69e638e 208 static bool prev = false;
hamohamo 0:f80bb69e638e 209 if(0 <= theta and theta < M_PI) gyro_1 = false;
hamohamo 0:f80bb69e638e 210 else gyro_1 = true;
hamohamo 0:f80bb69e638e 211 if(gyro_1 and !prev){
hamohamo 0:f80bb69e638e 212 if(gyro_2) g_pulse++;
hamohamo 0:f80bb69e638e 213 else if(g_pulse != 0) g_pulse--;
hamohamo 0:f80bb69e638e 214 }
hamohamo 0:f80bb69e638e 215 else if(!gyro_1 and prev){
hamohamo 0:f80bb69e638e 216 if(gyro_2) g_pulse--;
hamohamo 0:f80bb69e638e 217 else if(g_pulse != 0)g_pulse++;
hamohamo 0:f80bb69e638e 218 }
hamohamo 0:f80bb69e638e 219 prev = gyro_1;
hamohamo 0:f80bb69e638e 220 }
hamohamo 0:f80bb69e638e 221 void gyroB(double theta){
hamohamo 0:f80bb69e638e 222 static bool prev = false;
hamohamo 0:f80bb69e638e 223 if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false;
hamohamo 0:f80bb69e638e 224 else gyro_2 = true;
hamohamo 0:f80bb69e638e 225 if(gyro_2 and !prev){
hamohamo 0:f80bb69e638e 226 if(gyro_1) g_pulse--;
hamohamo 0:f80bb69e638e 227 else g_pulse++;
hamohamo 0:f80bb69e638e 228 }
hamohamo 0:f80bb69e638e 229 else if(!gyro_2 and prev){
hamohamo 0:f80bb69e638e 230 if(gyro_1) g_pulse++;
hamohamo 0:f80bb69e638e 231 else g_pulse--;
hamohamo 0:f80bb69e638e 232 }
hamohamo 0:f80bb69e638e 233 prev = gyro_2;
hamohamo 0:f80bb69e638e 234 }
hamohamo 0:f80bb69e638e 235
hamohamo 0:f80bb69e638e 236 bool checkMOT12(int rx_data, int &tx_data){
hamohamo 0:f80bb69e638e 237 MOT12 = true;
hamohamo 0:f80bb69e638e 238 return true;
hamohamo 0:f80bb69e638e 239 }
hamohamo 0:f80bb69e638e 240 bool checkMOT34(int rx_data, int &tx_data){
hamohamo 0:f80bb69e638e 241 MOT34 = true;
hamohamo 0:f80bb69e638e 242 return true;
hamohamo 0:f80bb69e638e 243 }
hamohamo 0:f80bb69e638e 244
hamohamo 1:aa9cc4250220 245 bool init(){
hamohamo 1:aa9cc4250220 246 wait(0.25);
hamohamo 0:f80bb69e638e 247 scrp.send1(255,5,1);
hamohamo 1:aa9cc4250220 248 wait(0.25);
hamohamo 1:aa9cc4250220 249 scrp.send1(255,6,1);
hamohamo 1:aa9cc4250220 250 return (!MOT12 or !MOT34);
hamohamo 4:0038da6cdc9a 251 }
hamohamo 4:0038da6cdc9a 252 void spinonce()
hamohamo 4:0038da6cdc9a 253 {
hamohamo 4:0038da6cdc9a 254 nh.spinOnce();
hamohamo 4:0038da6cdc9a 255 }
hamohamo 4:0038da6cdc9a 256
hamohamo 4:0038da6cdc9a 257 void getPixel(const std_msgs::Int16& pix){
hamohamo 4:0038da6cdc9a 258 pixel = pix.data;
hamohamo 4:0038da6cdc9a 259 }