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@12:ed227eb82214, 2021-11-13 (annotated)

Committer:
hamohamo
Date:
Sat Nov 13 05:11:01 2021 +0000
Revision:
12:ed227eb82214
Parent:
11:05c132be4f82 
sad

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