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