![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください
Dependencies: mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase
main.cpp@5:aee4416b0d4f, 2021-10-25 (annotated)
- Committer:
- minamikawa
- Date:
- Mon Oct 25 08:01:27 2021 +0000
- Revision:
- 5:aee4416b0d4f
- Parent:
- 4:0038da6cdc9a
- Child:
- 6:e7b9b1d46852
buffer size has changed
Who changed what in which revision?
User | Revision | Line number | New 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 |
minamikawa | 5:aee4416b0d4f | 48 | //ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); |
minamikawa | 5:aee4416b0d4f | 49 | ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,0x0807f800); |
hamohamo | 0:f80bb69e638e | 50 | Robot AKASHIKOSEN(50.8,25.4,322.5,259.75); |
hamohamo | 0:f80bb69e638e | 51 | Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02); |
hamohamo | 0:f80bb69e638e | 52 | |
minamikawa | 5:aee4416b0d4f | 53 | ros::Subscriber<std_msgs::Int16> sub("unti", &getPixel); |
minamikawa | 5:aee4416b0d4f | 54 | |
minamikawa | 5:aee4416b0d4f | 55 | int x=1; |
hamohamo | 4:0038da6cdc9a | 56 | |
hamohamo | 0:f80bb69e638e | 57 | int main(){ |
minamikawa | 5:aee4416b0d4f | 58 | |
minamikawa | 5:aee4416b0d4f | 59 | DigitalOut hoge(PB_2); |
minamikawa | 5:aee4416b0d4f | 60 | hoge = x; |
minamikawa | 5:aee4416b0d4f | 61 | |
hamohamo | 4:0038da6cdc9a | 62 | PwmOut ok(PC_6); |
hamohamo | 0:f80bb69e638e | 63 | |
hamohamo | 0:f80bb69e638e | 64 | /*--------------SETUP--------------*/ |
hamohamo | 4:0038da6cdc9a | 65 | |
hamohamo | 4:0038da6cdc9a | 66 | nh.getHardware()->setBaud(115200); |
hamohamo | 4:0038da6cdc9a | 67 | nh.initNode(); |
hamohamo | 4:0038da6cdc9a | 68 | nh.subscribe(sub); |
minamikawa | 5:aee4416b0d4f | 69 | rosspin.attach_us(&spinonce,1000); |
hamohamo | 4:0038da6cdc9a | 70 | |
hamohamo | 4:0038da6cdc9a | 71 | scrp.addCMD(7,checkMOT12); |
hamohamo | 4:0038da6cdc9a | 72 | scrp.addCMD(8,checkMOT34); |
hamohamo | 4:0038da6cdc9a | 73 | AKASHIKOSEN.setCWID(1,2,3,4); |
hamohamo | 4:0038da6cdc9a | 74 | AKASHIKOSEN.setSWID(5,6,7,8); |
hamohamo | 4:0038da6cdc9a | 75 | |
hamohamo | 4:0038da6cdc9a | 76 | RBT.addENC(PC_4,PA_13,512,4,1); |
hamohamo | 4:0038da6cdc9a | 77 | RBT.addENC(PA_14,PA_15,512,4,2); |
hamohamo | 4:0038da6cdc9a | 78 | RBT.addENC(PC_2,PC_3,512,4,3); |
hamohamo | 4:0038da6cdc9a | 79 | RBT.addENC(PC_10,PC_11,512,4,4); |
hamohamo | 4:0038da6cdc9a | 80 | RBT.addENC(PA_7,PA_6,512,4,5); |
hamohamo | 4:0038da6cdc9a | 81 | RBT.addENC(PA_9,PA_8,512,4,6); |
hamohamo | 4:0038da6cdc9a | 82 | RBT.addENC(PC_1,PC_0,512,4,7); |
hamohamo | 4:0038da6cdc9a | 83 | RBT.addENC(PC_5,PA_12,512,4,8); |
hamohamo | 0:f80bb69e638e | 84 | |
hamohamo | 4:0038da6cdc9a | 85 | RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 86 | RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 87 | RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 88 | RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 89 | 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 | 90 | 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 | 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 | RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 94 | RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 95 | RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 96 | RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5); |
hamohamo | 4:0038da6cdc9a | 97 | 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 | 98 | */ |
hamohamo | 4:0038da6cdc9a | 99 | MOT12 = false,MOT34 = false; |
hamohamo | 4:0038da6cdc9a | 100 | |
hamohamo | 4:0038da6cdc9a | 101 | while(init()); |
hamohamo | 4:0038da6cdc9a | 102 | printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34); |
hamohamo | 4:0038da6cdc9a | 103 | route_read(); |
hamohamo | 4:0038da6cdc9a | 104 | |
hamohamo | 4:0038da6cdc9a | 105 | wait(1); |
hamohamo | 4:0038da6cdc9a | 106 | ok = 1.0; |
hamohamo | 0:f80bb69e638e | 107 | |
hamohamo | 4:0038da6cdc9a | 108 | bool setup = false; |
minamikawa | 5:aee4416b0d4f | 109 | bool once = false; |
hamohamo | 4:0038da6cdc9a | 110 | if(!setup){ |
hamohamo | 4:0038da6cdc9a | 111 | bno.reset(); |
hamohamo | 4:0038da6cdc9a | 112 | bno.setmode(OPERATION_MODE_IMUPLUS); |
hamohamo | 4:0038da6cdc9a | 113 | setup = true; |
hamohamo | 0:f80bb69e638e | 114 | } |
hamohamo | 4:0038da6cdc9a | 115 | |
hamohamo | 4:0038da6cdc9a | 116 | RBT.START(); |
hamohamo | 4:0038da6cdc9a | 117 | RBT.setPosition(0.0,0.0,0.0); |
hamohamo | 4:0038da6cdc9a | 118 | /*--------------LOOP--------------*/ |
hamohamo | 4:0038da6cdc9a | 119 | while(true){ |
minamikawa | 5:aee4416b0d4f | 120 | hoge = x; |
hamohamo | 4:0038da6cdc9a | 121 | pos = RBT.getStatus(true); |
hamohamo | 4:0038da6cdc9a | 122 | bno.get_angles(); |
hamohamo | 4:0038da6cdc9a | 123 | gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); |
hamohamo | 4:0038da6cdc9a | 124 | gyroA(gyro); |
hamohamo | 4:0038da6cdc9a | 125 | gyroB(gyro); |
hamohamo | 4:0038da6cdc9a | 126 | |
hamohamo | 4:0038da6cdc9a | 127 | theta = 2.0*M_PI*(int)(g_pulse/4.0); |
hamohamo | 4:0038da6cdc9a | 128 | if(g_pulse > 0.0) theta += gyro; |
hamohamo | 4:0038da6cdc9a | 129 | else if(g_pulse < 0.0) theta += gyro-2.0*M_PI; |
hamohamo | 4:0038da6cdc9a | 130 | else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro; |
hamohamo | 4:0038da6cdc9a | 131 | else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI; |
minamikawa | 5:aee4416b0d4f | 132 | if(!once) { |
minamikawa | 5:aee4416b0d4f | 133 | RBT.setPosition(0.0,0.0,0.0); |
minamikawa | 5:aee4416b0d4f | 134 | once = true; |
minamikawa | 5:aee4416b0d4f | 135 | } |
hamohamo | 4:0038da6cdc9a | 136 | RBT.setPosition(pos.x,pos.y,theta); |
hamohamo | 4:0038da6cdc9a | 137 | |
hamohamo | 4:0038da6cdc9a | 138 | //RBT.setVelocity(0.0,0.0,0.0); |
hamohamo | 4:0038da6cdc9a | 139 | if(n<POINTS and |
hamohamo | 4:0038da6cdc9a | 140 | ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000 and n != ISLAND) or |
hamohamo | 4:0038da6cdc9a | 141 | ( incircle(pos,NextP,10.0) and n == 1000 ) or |
minamikawa | 5:aee4416b0d4f | 142 | ( 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) |
hamohamo | 4:0038da6cdc9a | 143 | ) |
hamohamo | 4:0038da6cdc9a | 144 | ) |
hamohamo | 4:0038da6cdc9a | 145 | { |
hamohamo | 4:0038da6cdc9a | 146 | n++; |
hamohamo | 4:0038da6cdc9a | 147 | getPoint(&NextP,n); |
hamohamo | 4:0038da6cdc9a | 148 | NextP.theta -= M_PI/2.0; |
hamohamo | 4:0038da6cdc9a | 149 | getVelocity(&v,n-1); |
hamohamo | 4:0038da6cdc9a | 150 | if(n < POINTS)getPoint(&subP,n+1); |
hamohamo | 4:0038da6cdc9a | 151 | if(v > 0.0) v *= 1.5; |
hamohamo | 4:0038da6cdc9a | 152 | if(v >= 1400) v = 1400; |
hamohamo | 4:0038da6cdc9a | 153 | //v = 20.0; |
hamohamo | 4:0038da6cdc9a | 154 | if(n == POINTS) v = 20.0; |
hamohamo | 4:0038da6cdc9a | 155 | printf("%d:pass\n",n); |
hamohamo | 4:0038da6cdc9a | 156 | } |
hamohamo | 4:0038da6cdc9a | 157 | |
hamohamo | 4:0038da6cdc9a | 158 | double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x); |
hamohamo | 4:0038da6cdc9a | 159 | RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); |
hamohamo | 4:0038da6cdc9a | 160 | vel.theta = RBT.PIDs[5]->getmv(); |
hamohamo | 4:0038da6cdc9a | 161 | |
minamikawa | 5:aee4416b0d4f | 162 | if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 10){ |
hamohamo | 4:0038da6cdc9a | 163 | RBT.PIDs[0]->Update(pixel,0,0.02,false); |
minamikawa | 5:aee4416b0d4f | 164 | v = 0.0; |
hamohamo | 4:0038da6cdc9a | 165 | vel.theta = RBT.PIDs[0]->getmv(); |
hamohamo | 4:0038da6cdc9a | 166 | } |
minamikawa | 5:aee4416b0d4f | 167 | else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9)*/;} |
hamohamo | 4:0038da6cdc9a | 168 | |
hamohamo | 4:0038da6cdc9a | 169 | if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0; |
hamohamo | 4:0038da6cdc9a | 170 | if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) vel.theta = 0.0; |
hamohamo | 4:0038da6cdc9a | 171 | |
hamohamo | 4:0038da6cdc9a | 172 | vel.x = v*cos(etheta); |
hamohamo | 4:0038da6cdc9a | 173 | vel.y = v*sin(etheta); |
hamohamo | 4:0038da6cdc9a | 174 | |
hamohamo | 4:0038da6cdc9a | 175 | RBT.sendVelocity(vel.x,vel.y,vel.theta); |
hamohamo | 4:0038da6cdc9a | 176 | printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n); |
minamikawa | 5:aee4416b0d4f | 177 | nh.spinOnce(); |
hamohamo | 4:0038da6cdc9a | 178 | RBT.LOOP(); |
hamohamo | 4:0038da6cdc9a | 179 | } |
hamohamo | 0:f80bb69e638e | 180 | } |
hamohamo | 0:f80bb69e638e | 181 | |
hamohamo | 0:f80bb69e638e | 182 | void route_read() { |
hamohamo | 0:f80bb69e638e | 183 | static double x, y, v, theta_; |
hamohamo | 0:f80bb69e638e | 184 | wait(1); |
hamohamo | 0:f80bb69e638e | 185 | printf("Hello World!\n"); |
hamohamo | 0:f80bb69e638e | 186 | wait(0.1); |
hamohamo | 0:f80bb69e638e | 187 | FILE *fp; |
hamohamo | 0:f80bb69e638e | 188 | do { |
hamohamo | 0:f80bb69e638e | 189 | fp = fopen("/sd/route.txt", "r"); |
hamohamo | 0:f80bb69e638e | 190 | wait(0.2); |
hamohamo | 0:f80bb69e638e | 191 | } while(fp==NULL); |
hamohamo | 0:f80bb69e638e | 192 | printf("Good!\n"); |
hamohamo | 0:f80bb69e638e | 193 | while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) { |
hamohamo | 0:f80bb69e638e | 194 | printf("%lf %lf %lf %lf\n",x, y, v, theta_); |
hamohamo | 1:aa9cc4250220 | 195 | x_info.push_back(x*2.0); |
hamohamo | 1:aa9cc4250220 | 196 | y_info.push_back(y*1.5); |
hamohamo | 0:f80bb69e638e | 197 | v_info.push_back(v); |
hamohamo | 0:f80bb69e638e | 198 | theta_info.push_back(theta_); |
hamohamo | 0:f80bb69e638e | 199 | } |
hamohamo | 0:f80bb69e638e | 200 | printf("checking...\n"); |
hamohamo | 0:f80bb69e638e | 201 | for(int k=0;k<x_info.size();k++){ |
hamohamo | 0:f80bb69e638e | 202 | printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]); |
hamohamo | 0:f80bb69e638e | 203 | } |
hamohamo | 0:f80bb69e638e | 204 | fclose(fp); |
hamohamo | 0:f80bb69e638e | 205 | } |
hamohamo | 0:f80bb69e638e | 206 | |
hamohamo | 0:f80bb69e638e | 207 | void getPoint(Position *p,int n){ |
hamohamo | 0:f80bb69e638e | 208 | p->x = x_info[n]; |
hamohamo | 0:f80bb69e638e | 209 | p->y = y_info[n]; |
hamohamo | 0:f80bb69e638e | 210 | p->theta = theta_info[n]; |
hamohamo | 0:f80bb69e638e | 211 | } |
hamohamo | 0:f80bb69e638e | 212 | |
hamohamo | 0:f80bb69e638e | 213 | void getVelocity(double *v,int n){ |
hamohamo | 0:f80bb69e638e | 214 | *v = v_info[n]; |
hamohamo | 0:f80bb69e638e | 215 | } |
hamohamo | 0:f80bb69e638e | 216 | |
hamohamo | 0:f80bb69e638e | 217 | bool incircle(Position pos,Position target,double error){ |
hamohamo | 0:f80bb69e638e | 218 | return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error); |
hamohamo | 0:f80bb69e638e | 219 | } |
hamohamo | 0:f80bb69e638e | 220 | void gyroA(double theta){ |
hamohamo | 0:f80bb69e638e | 221 | static bool prev = false; |
hamohamo | 0:f80bb69e638e | 222 | if(0 <= theta and theta < M_PI) gyro_1 = false; |
hamohamo | 0:f80bb69e638e | 223 | else gyro_1 = true; |
hamohamo | 0:f80bb69e638e | 224 | if(gyro_1 and !prev){ |
hamohamo | 0:f80bb69e638e | 225 | if(gyro_2) g_pulse++; |
hamohamo | 0:f80bb69e638e | 226 | else if(g_pulse != 0) g_pulse--; |
hamohamo | 0:f80bb69e638e | 227 | } |
hamohamo | 0:f80bb69e638e | 228 | else if(!gyro_1 and prev){ |
hamohamo | 0:f80bb69e638e | 229 | if(gyro_2) g_pulse--; |
hamohamo | 0:f80bb69e638e | 230 | else if(g_pulse != 0)g_pulse++; |
hamohamo | 0:f80bb69e638e | 231 | } |
hamohamo | 0:f80bb69e638e | 232 | prev = gyro_1; |
hamohamo | 0:f80bb69e638e | 233 | } |
hamohamo | 0:f80bb69e638e | 234 | void gyroB(double theta){ |
hamohamo | 0:f80bb69e638e | 235 | static bool prev = false; |
hamohamo | 0:f80bb69e638e | 236 | if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false; |
hamohamo | 0:f80bb69e638e | 237 | else gyro_2 = true; |
hamohamo | 0:f80bb69e638e | 238 | if(gyro_2 and !prev){ |
hamohamo | 0:f80bb69e638e | 239 | if(gyro_1) g_pulse--; |
hamohamo | 0:f80bb69e638e | 240 | else g_pulse++; |
hamohamo | 0:f80bb69e638e | 241 | } |
hamohamo | 0:f80bb69e638e | 242 | else if(!gyro_2 and prev){ |
hamohamo | 0:f80bb69e638e | 243 | if(gyro_1) g_pulse++; |
hamohamo | 0:f80bb69e638e | 244 | else g_pulse--; |
hamohamo | 0:f80bb69e638e | 245 | } |
hamohamo | 0:f80bb69e638e | 246 | prev = gyro_2; |
hamohamo | 0:f80bb69e638e | 247 | } |
hamohamo | 0:f80bb69e638e | 248 | |
hamohamo | 0:f80bb69e638e | 249 | bool checkMOT12(int rx_data, int &tx_data){ |
hamohamo | 0:f80bb69e638e | 250 | MOT12 = true; |
hamohamo | 0:f80bb69e638e | 251 | return true; |
hamohamo | 0:f80bb69e638e | 252 | } |
hamohamo | 0:f80bb69e638e | 253 | bool checkMOT34(int rx_data, int &tx_data){ |
hamohamo | 0:f80bb69e638e | 254 | MOT34 = true; |
hamohamo | 0:f80bb69e638e | 255 | return true; |
hamohamo | 0:f80bb69e638e | 256 | } |
hamohamo | 0:f80bb69e638e | 257 | |
hamohamo | 1:aa9cc4250220 | 258 | bool init(){ |
hamohamo | 1:aa9cc4250220 | 259 | wait(0.25); |
hamohamo | 0:f80bb69e638e | 260 | scrp.send1(255,5,1); |
hamohamo | 1:aa9cc4250220 | 261 | wait(0.25); |
hamohamo | 1:aa9cc4250220 | 262 | scrp.send1(255,6,1); |
minamikawa | 5:aee4416b0d4f | 263 | printf("aaaa"); |
hamohamo | 1:aa9cc4250220 | 264 | return (!MOT12 or !MOT34); |
hamohamo | 4:0038da6cdc9a | 265 | } |
hamohamo | 4:0038da6cdc9a | 266 | void spinonce() |
hamohamo | 4:0038da6cdc9a | 267 | { |
hamohamo | 4:0038da6cdc9a | 268 | nh.spinOnce(); |
hamohamo | 4:0038da6cdc9a | 269 | } |
hamohamo | 4:0038da6cdc9a | 270 | |
hamohamo | 4:0038da6cdc9a | 271 | void getPixel(const std_msgs::Int16& pix){ |
hamohamo | 4:0038da6cdc9a | 272 | pixel = pix.data; |
minamikawa | 5:aee4416b0d4f | 273 | if (pixel > 0){ |
minamikawa | 5:aee4416b0d4f | 274 | x = 0; |
minamikawa | 5:aee4416b0d4f | 275 | } else { |
minamikawa | 5:aee4416b0d4f | 276 | x = 1; |
hamohamo | 4:0038da6cdc9a | 277 | } |
minamikawa | 5:aee4416b0d4f | 278 | } |