hidaka sato / Mbed 2 deprecated WRS_mechanamu_test

For our Robot

Dependencies:   JY901 Make_Sequencer_3 PID PacketSerial QEI_simple SensorBar mbed

main.cpp@0:08b48d8e6eab, 2018-11-14 (annotated)

Committer:
sgrsn
Date:
Wed Nov 14 02:23:08 2018 +0000
Revision:
0:08b48d8e6eab
First Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:08b48d8e6eab 1 #include "mbed.h"
sgrsn 0:08b48d8e6eab 2 #include "JY901.h"
sgrsn 0:08b48d8e6eab 3 #include "QEI.h"
sgrsn 0:08b48d8e6eab 4 #include "PID.h"
sgrsn 0:08b48d8e6eab 5 #include "SensorBar.h"
sgrsn 0:08b48d8e6eab 6 #include "pitches.h"
sgrsn 0:08b48d8e6eab 7 #include "PacketSerial.h"
sgrsn 0:08b48d8e6eab 8 #include "MakeSequencer.h"
sgrsn 0:08b48d8e6eab 9
sgrsn 0:08b48d8e6eab 10 // IIC Address
sgrsn 0:08b48d8e6eab 11 #define Arduino_Address 0x20<<1
sgrsn 0:08b48d8e6eab 12 #define SX1509_ADDRESS 0x3E<<1
sgrsn 0:08b48d8e6eab 13
sgrsn 0:08b48d8e6eab 14 // Arduino Register Map
sgrsn 0:08b48d8e6eab 15 #define TARGET_X 0x10
sgrsn 0:08b48d8e6eab 16 #define TARGET_Y 0x11
sgrsn 0:08b48d8e6eab 17 #define TARGET_Z 0x12
sgrsn 0:08b48d8e6eab 18 #define TARGET_R 0x13
sgrsn 0:08b48d8e6eab 19 #define TARGET_H 0x14
sgrsn 0:08b48d8e6eab 20 #define SERVO_ENABLE 0x20
sgrsn 0:08b48d8e6eab 21 #define SERVO_DELAY 0x21
sgrsn 0:08b48d8e6eab 22
sgrsn 0:08b48d8e6eab 23 //RaspberryPi Register Map
sgrsn 0:08b48d8e6eab 24 #define SENSOR_ENABLE 0x00
sgrsn 0:08b48d8e6eab 25 #define FRONT_OBJ_DISTANCE 0x01
sgrsn 0:08b48d8e6eab 26 #define FRONT_OBJ_POSITION 0x02
sgrsn 0:08b48d8e6eab 27 #define BACK_DISTANCE1 0x03
sgrsn 0:08b48d8e6eab 28 #define BACK_DISTANCE2 0x04
sgrsn 0:08b48d8e6eab 29 #define RIGHT_DISTANCE1 0x05
sgrsn 0:08b48d8e6eab 30 #define RIGHT_DISTANCE2 0x06
sgrsn 0:08b48d8e6eab 31 #define LEFT_DISTANCE1 0x07
sgrsn 0:08b48d8e6eab 32 #define LEFT_DISTANCE2 0x08
sgrsn 0:08b48d8e6eab 33
sgrsn 0:08b48d8e6eab 34
sgrsn 0:08b48d8e6eab 35 float pi = 3.14159265359;
sgrsn 0:08b48d8e6eab 36 float wheel_r = 38.0/2.0; //接地エンコーダのオムニ
sgrsn 0:08b48d8e6eab 37 float wheel_x_g = 50.0; //x軸接地エンコーダの重心からのずれ
sgrsn 0:08b48d8e6eab 38 float DEG_TO_RAD = pi / 180;
sgrsn 0:08b48d8e6eab 39 float RAD_TO_DEG = 180.0 / pi;
sgrsn 0:08b48d8e6eab 40
sgrsn 0:08b48d8e6eab 41 const int Gcode_size = 6; //gコードの種類の数
sgrsn 0:08b48d8e6eab 42
sgrsn 0:08b48d8e6eab 43 I2C i2c(p28, p27);
sgrsn 0:08b48d8e6eab 44 Timer timer;
sgrsn 0:08b48d8e6eab 45 Timer timer2;
sgrsn 0:08b48d8e6eab 46 QEI RE[] = { QEI(p26, p25, 100, &timer), QEI(p23, p24, 100, &timer) };
sgrsn 0:08b48d8e6eab 47 SensorBar line(&i2c, SX1509_ADDRESS);
sgrsn 0:08b48d8e6eab 48 PwmOut buzzer(p21);
sgrsn 0:08b48d8e6eab 49 DigitalIn button(p12);
sgrsn 0:08b48d8e6eab 50 BusIn modeChange(p15, p16, p17);
sgrsn 0:08b48d8e6eab 51 BusOut LED(p18, p19, p20);
sgrsn 0:08b48d8e6eab 52 LocalFileSystem local("local");
sgrsn 0:08b48d8e6eab 53
sgrsn 0:08b48d8e6eab 54 int32_t Register[10] = {};
sgrsn 0:08b48d8e6eab 55 PacketSerial Raspi(USBTX, USBRX, Register, 115200);
sgrsn 0:08b48d8e6eab 56
sgrsn 0:08b48d8e6eab 57 PID pid_x(&timer);
sgrsn 0:08b48d8e6eab 58 PID pid_y(&timer);
sgrsn 0:08b48d8e6eab 59 PID pid_yaw(&timer);
sgrsn 0:08b48d8e6eab 60 JY901 jy901(&i2c, &timer2);
sgrsn 0:08b48d8e6eab 61
sgrsn 0:08b48d8e6eab 62 bool writeSomeData(char addr, char reg, int32_t data ,int size);
sgrsn 0:08b48d8e6eab 63
sgrsn 0:08b48d8e6eab 64 int PATH[1500][3] = {};
sgrsn 0:08b48d8e6eab 65 //int *PATH = new int[1000][3];
sgrsn 0:08b48d8e6eab 66 int makePath(int targetX, int targetY, int targetR, int x, int y, int angleZ);
sgrsn 0:08b48d8e6eab 67
sgrsn 0:08b48d8e6eab 68 void alarm(int n = 5);
sgrsn 0:08b48d8e6eab 69 void motorStop();
sgrsn 0:08b48d8e6eab 70 float sign(float val);
sgrsn 0:08b48d8e6eab 71
sgrsn 0:08b48d8e6eab 72 void robotGuide(FILE *fp)
sgrsn 0:08b48d8e6eab 73 {
sgrsn 0:08b48d8e6eab 74 MakeSequencer code(fp);
sgrsn 0:08b48d8e6eab 75 const int seq_size = code.getGcodeSize();
sgrsn 0:08b48d8e6eab 76 int Gcode[Gcode_size] = {};
sgrsn 0:08b48d8e6eab 77 int awayCode[Gcode_size] = {};
sgrsn 0:08b48d8e6eab 78 int awayFlag = 0; //0:awayなし, 1:awayあり, 2,前方に物体なし, 3:通過
sgrsn 0:08b48d8e6eab 79 int objectSide = 0;
sgrsn 0:08b48d8e6eab 80 int laserSW = 0;
sgrsn 0:08b48d8e6eab 81 int laserSWFlag[2] = {};
sgrsn 0:08b48d8e6eab 82 int deltaX = 0, deltaY = 0;
sgrsn 0:08b48d8e6eab 83 code.getGcode(0, Gcode_size, Gcode);
sgrsn 0:08b48d8e6eab 84 pid_x.setParameter(2.0, 0.57851);//(6.0, 0.57851);
sgrsn 0:08b48d8e6eab 85 pid_y.setParameter(2.0, 0.58);//(2.0, 0.7155);
sgrsn 0:08b48d8e6eab 86 pid_yaw.setParameter(7, 0.6579);
sgrsn 0:08b48d8e6eab 87 float x = 0, y = 0, angleZ = 0;
sgrsn 0:08b48d8e6eab 88 int objectX = 0, objectY = 2000;
sgrsn 0:08b48d8e6eab 89 int seq = 0;
sgrsn 0:08b48d8e6eab 90 int path = 0;
sgrsn 0:08b48d8e6eab 91 int path_size = makePath(Gcode[seq], Gcode[seq], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 92
sgrsn 0:08b48d8e6eab 93 float vx = 0, vy = 0, vo = 0;
sgrsn 0:08b48d8e6eab 94
sgrsn 0:08b48d8e6eab 95 while(seq < seq_size)
sgrsn 0:08b48d8e6eab 96 {
sgrsn 0:08b48d8e6eab 97 //自己位置推定
sgrsn 0:08b48d8e6eab 98 angleZ = jy901.calculateAngleOnlyGyro();
sgrsn 0:08b48d8e6eab 99 float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 100 float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 101 x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 102 y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 103
sgrsn 0:08b48d8e6eab 104 //障害物相対位置取得
sgrsn 0:08b48d8e6eab 105 objectX = Register[FRONT_OBJ_POSITION];
sgrsn 0:08b48d8e6eab 106 objectY = Register[FRONT_OBJ_DISTANCE];
sgrsn 0:08b48d8e6eab 107
sgrsn 0:08b48d8e6eab 108 //壁除外処理
sgrsn 0:08b48d8e6eab 109 if( (y > 1000/* && angleZ < 10 && angleZ > -10*/) || (y < 500 && angleZ > 170 && angleZ < 190) )
sgrsn 0:08b48d8e6eab 110 {
sgrsn 0:08b48d8e6eab 111 }
sgrsn 0:08b48d8e6eab 112
sgrsn 0:08b48d8e6eab 113 //障害物発見
sgrsn 0:08b48d8e6eab 114 else if(objectY < 250)
sgrsn 0:08b48d8e6eab 115 {
sgrsn 0:08b48d8e6eab 116 if(awayFlag != 1)
sgrsn 0:08b48d8e6eab 117 {
sgrsn 0:08b48d8e6eab 118 motorStop();
sgrsn 0:08b48d8e6eab 119 wait_ms(500);
sgrsn 0:08b48d8e6eab 120 alarm();
sgrsn 0:08b48d8e6eab 121 //waitがあるのでジャイロのリセット
sgrsn 0:08b48d8e6eab 122 jy901.reset();
sgrsn 0:08b48d8e6eab 123 //一応pidもリセットしとく
sgrsn 0:08b48d8e6eab 124 pid_x.reset();
sgrsn 0:08b48d8e6eab 125 pid_y.reset();
sgrsn 0:08b48d8e6eab 126 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 127 //経路の再作成
sgrsn 0:08b48d8e6eab 128 //左側
sgrsn 0:08b48d8e6eab 129 if(objectX*cos(angleZ*DEG_TO_RAD) > 0 && angleZ < 10 && angleZ > -10) deltaX = -100, objectSide = 3;
sgrsn 0:08b48d8e6eab 130 //右側
sgrsn 0:08b48d8e6eab 131 else if(objectX*cos(angleZ*DEG_TO_RAD) <= 0 && angleZ < 10 && angleZ > -10) deltaX = 100, objectSide = 2;
sgrsn 0:08b48d8e6eab 132 awayCode[0] = x + deltaX;
sgrsn 0:08b48d8e6eab 133 awayCode[1] = y; //yは維持
sgrsn 0:08b48d8e6eab 134 awayCode[2] = angleZ; //角度も維持
sgrsn 0:08b48d8e6eab 135 awayFlag = 1;
sgrsn 0:08b48d8e6eab 136 path_size = makePath(awayCode[0], awayCode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 137 path = 0;
sgrsn 0:08b48d8e6eab 138 }
sgrsn 0:08b48d8e6eab 139 }
sgrsn 0:08b48d8e6eab 140 else if(awayFlag == 2)
sgrsn 0:08b48d8e6eab 141 {
sgrsn 0:08b48d8e6eab 142 deltaX = 0;
sgrsn 0:08b48d8e6eab 143 //正方向
sgrsn 0:08b48d8e6eab 144 if((Gcode[1]-y)*cos(angleZ*DEG_TO_RAD) > 0) deltaY = 100;
sgrsn 0:08b48d8e6eab 145 //負方向
sgrsn 0:08b48d8e6eab 146 if((Gcode[1]-y)*cos(angleZ*DEG_TO_RAD) < 0) deltaY = -100;
sgrsn 0:08b48d8e6eab 147 awayCode[0] = x;
sgrsn 0:08b48d8e6eab 148 awayCode[1] = Gcode[1];//y+deltaY;
sgrsn 0:08b48d8e6eab 149 awayCode[2] = angleZ; //角度も維持
sgrsn 0:08b48d8e6eab 150 path_size = makePath(awayCode[0], awayCode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 151 path = 0;
sgrsn 0:08b48d8e6eab 152 awayFlag = 3;
sgrsn 0:08b48d8e6eab 153 }
sgrsn 0:08b48d8e6eab 154 //左側
sgrsn 0:08b48d8e6eab 155 if(objectSide == 2)
sgrsn 0:08b48d8e6eab 156 {
sgrsn 0:08b48d8e6eab 157 if(Register[LEFT_DISTANCE1] < 200)
sgrsn 0:08b48d8e6eab 158 {
sgrsn 0:08b48d8e6eab 159 if(laserSWFlag[0] == 1)
sgrsn 0:08b48d8e6eab 160 {
sgrsn 0:08b48d8e6eab 161 laserSW++;
sgrsn 0:08b48d8e6eab 162 laserSWFlag[0] = 0;
sgrsn 0:08b48d8e6eab 163 }
sgrsn 0:08b48d8e6eab 164 }
sgrsn 0:08b48d8e6eab 165 else
sgrsn 0:08b48d8e6eab 166 {
sgrsn 0:08b48d8e6eab 167 laserSWFlag[0] = 1;
sgrsn 0:08b48d8e6eab 168 }
sgrsn 0:08b48d8e6eab 169 if(Register[LEFT_DISTANCE2] < 200)
sgrsn 0:08b48d8e6eab 170 {
sgrsn 0:08b48d8e6eab 171 if(laserSWFlag[1] == 1)
sgrsn 0:08b48d8e6eab 172 {
sgrsn 0:08b48d8e6eab 173 laserSW++;
sgrsn 0:08b48d8e6eab 174 laserSWFlag[1] = 0;
sgrsn 0:08b48d8e6eab 175 }
sgrsn 0:08b48d8e6eab 176 }
sgrsn 0:08b48d8e6eab 177 else
sgrsn 0:08b48d8e6eab 178 {
sgrsn 0:08b48d8e6eab 179 laserSWFlag[1] = 1;
sgrsn 0:08b48d8e6eab 180 }
sgrsn 0:08b48d8e6eab 181 }
sgrsn 0:08b48d8e6eab 182 if(objectSide == 3)
sgrsn 0:08b48d8e6eab 183 {
sgrsn 0:08b48d8e6eab 184 if(Register[RIGHT_DISTANCE1] < 200)
sgrsn 0:08b48d8e6eab 185 {
sgrsn 0:08b48d8e6eab 186 if(laserSWFlag[0] == 1)
sgrsn 0:08b48d8e6eab 187 {
sgrsn 0:08b48d8e6eab 188 laserSW++;
sgrsn 0:08b48d8e6eab 189 laserSWFlag[0] = 0;
sgrsn 0:08b48d8e6eab 190 }
sgrsn 0:08b48d8e6eab 191 }
sgrsn 0:08b48d8e6eab 192 else
sgrsn 0:08b48d8e6eab 193 {
sgrsn 0:08b48d8e6eab 194 laserSWFlag[0] = 1;
sgrsn 0:08b48d8e6eab 195 }
sgrsn 0:08b48d8e6eab 196 if(Register[RIGHT_DISTANCE2] < 200)
sgrsn 0:08b48d8e6eab 197 {
sgrsn 0:08b48d8e6eab 198 if(laserSWFlag[1] == 1)
sgrsn 0:08b48d8e6eab 199 {
sgrsn 0:08b48d8e6eab 200 laserSW++;
sgrsn 0:08b48d8e6eab 201 laserSWFlag[1] = 0;
sgrsn 0:08b48d8e6eab 202 }
sgrsn 0:08b48d8e6eab 203 }
sgrsn 0:08b48d8e6eab 204 else
sgrsn 0:08b48d8e6eab 205 {
sgrsn 0:08b48d8e6eab 206 laserSWFlag[1] = 1;
sgrsn 0:08b48d8e6eab 207 }
sgrsn 0:08b48d8e6eab 208 }
sgrsn 0:08b48d8e6eab 209
sgrsn 0:08b48d8e6eab 210 /*if(laserSW < 2 && awayFlag == 3)
sgrsn 0:08b48d8e6eab 211 {
sgrsn 0:08b48d8e6eab 212 awayFlag = 2;
sgrsn 0:08b48d8e6eab 213 }*/
sgrsn 0:08b48d8e6eab 214 /*if(laserSW >= 2 && awayFlag == 3)
sgrsn 0:08b48d8e6eab 215 {
sgrsn 0:08b48d8e6eab 216 laserSW = 0;
sgrsn 0:08b48d8e6eab 217 awayCode[0] = Gcode[0];
sgrsn 0:08b48d8e6eab 218 awayCode[1] = Gcode[1]; //yは維持
sgrsn 0:08b48d8e6eab 219 awayCode[2] = angleZ; //角度も維持
sgrsn 0:08b48d8e6eab 220 awayFlag = 4;
sgrsn 0:08b48d8e6eab 221 path_size = makePath(awayCode[0], awayCode[1], awayCode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 222 }*/
sgrsn 0:08b48d8e6eab 223 if(laserSW >= 2 && awayFlag == 3)
sgrsn 0:08b48d8e6eab 224 {
sgrsn 0:08b48d8e6eab 225 laserSW = 0;
sgrsn 0:08b48d8e6eab 226 awayCode[0] = x;
sgrsn 0:08b48d8e6eab 227 awayCode[1] = y+200;
sgrsn 0:08b48d8e6eab 228 awayCode[2] = angleZ;
sgrsn 0:08b48d8e6eab 229 awayFlag = 4;
sgrsn 0:08b48d8e6eab 230 path_size = makePath(awayCode[0], awayCode[1], awayCode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 231 }
sgrsn 0:08b48d8e6eab 232
sgrsn 0:08b48d8e6eab 233 //目標位置取得
sgrsn 0:08b48d8e6eab 234 int targetX = PATH[path][0];
sgrsn 0:08b48d8e6eab 235 int targetY = PATH[path][1];
sgrsn 0:08b48d8e6eab 236 int targetR = PATH[path][2];
sgrsn 0:08b48d8e6eab 237
sgrsn 0:08b48d8e6eab 238 //経路更新
sgrsn 0:08b48d8e6eab 239 path++;
sgrsn 0:08b48d8e6eab 240 if(path >= path_size)path = path_size-1;
sgrsn 0:08b48d8e6eab 241
sgrsn 0:08b48d8e6eab 242 //目標速度ベクトル計算
sgrsn 0:08b48d8e6eab 243 float xd = targetX - x;
sgrsn 0:08b48d8e6eab 244 float yd = targetY - y;
sgrsn 0:08b48d8e6eab 245 float r = sqrt(xd*xd + yd*yd);
sgrsn 0:08b48d8e6eab 246 float fai = atan2(yd, xd) - angleZ*DEG_TO_RAD;
sgrsn 0:08b48d8e6eab 247 float target_dx = r*cos(fai);
sgrsn 0:08b48d8e6eab 248 float target_dy = r*sin(fai);
sgrsn 0:08b48d8e6eab 249 vx = pid_x.controlPID(target_dx, 0);
sgrsn 0:08b48d8e6eab 250 vy = pid_y.controlPID(target_dy, 0);
sgrsn 0:08b48d8e6eab 251 vo = pid_yaw.controlPID(targetR, angleZ);
sgrsn 0:08b48d8e6eab 252
sgrsn 0:08b48d8e6eab 253 if(awayFlag == 1)
sgrsn 0:08b48d8e6eab 254 {
sgrsn 0:08b48d8e6eab 255 if(abs(awayCode[0]-x)<5 && abs(awayCode[1]-y)<5 && abs(awayCode[2]-angleZ)<2 && abs(vo)<2)
sgrsn 0:08b48d8e6eab 256 {
sgrsn 0:08b48d8e6eab 257 //車輪の停止(念のため5回)
sgrsn 0:08b48d8e6eab 258 for(int _ = 0; _ < 5; _++)
sgrsn 0:08b48d8e6eab 259 {
sgrsn 0:08b48d8e6eab 260 writeSomeData(Arduino_Address, TARGET_X, 0, 4);
sgrsn 0:08b48d8e6eab 261 wait_ms(1);
sgrsn 0:08b48d8e6eab 262 writeSomeData(Arduino_Address, TARGET_Y, 0, 4);
sgrsn 0:08b48d8e6eab 263 wait_ms(1);
sgrsn 0:08b48d8e6eab 264 writeSomeData(Arduino_Address, TARGET_R, 0, 4);
sgrsn 0:08b48d8e6eab 265 wait_ms(1);
sgrsn 0:08b48d8e6eab 266 }
sgrsn 0:08b48d8e6eab 267 awayFlag = 2;
sgrsn 0:08b48d8e6eab 268 path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 269 path = 0;
sgrsn 0:08b48d8e6eab 270
sgrsn 0:08b48d8e6eab 271 //waitがあるのでジャイロのリセット
sgrsn 0:08b48d8e6eab 272 jy901.reset();
sgrsn 0:08b48d8e6eab 273 //一応pidもリセットしとく
sgrsn 0:08b48d8e6eab 274 pid_x.reset();
sgrsn 0:08b48d8e6eab 275 pid_y.reset();
sgrsn 0:08b48d8e6eab 276 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 277 }
sgrsn 0:08b48d8e6eab 278 else
sgrsn 0:08b48d8e6eab 279 {
sgrsn 0:08b48d8e6eab 280 writeSomeData(Arduino_Address, TARGET_X, vx, 4);
sgrsn 0:08b48d8e6eab 281 writeSomeData(Arduino_Address, TARGET_Y, vy, 4);
sgrsn 0:08b48d8e6eab 282 writeSomeData(Arduino_Address, TARGET_R, vo, 4);
sgrsn 0:08b48d8e6eab 283 }
sgrsn 0:08b48d8e6eab 284 }
sgrsn 0:08b48d8e6eab 285 else if(awayFlag == 4)
sgrsn 0:08b48d8e6eab 286 {
sgrsn 0:08b48d8e6eab 287 if(abs(awayCode[0]-x)<5 && abs(awayCode[1]-y)<5 && abs(awayCode[2]-angleZ)<2 && abs(vo)<2)
sgrsn 0:08b48d8e6eab 288 {
sgrsn 0:08b48d8e6eab 289 //車輪の停止(念のため5回)
sgrsn 0:08b48d8e6eab 290 motorStop();
sgrsn 0:08b48d8e6eab 291 awayFlag = 0;
sgrsn 0:08b48d8e6eab 292 path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 293 path = 0;
sgrsn 0:08b48d8e6eab 294 //waitがあるのでジャイロのリセット
sgrsn 0:08b48d8e6eab 295 jy901.reset();
sgrsn 0:08b48d8e6eab 296 //一応pidもリセットしとく
sgrsn 0:08b48d8e6eab 297 pid_x.reset();
sgrsn 0:08b48d8e6eab 298 pid_y.reset();
sgrsn 0:08b48d8e6eab 299 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 300 }
sgrsn 0:08b48d8e6eab 301 else
sgrsn 0:08b48d8e6eab 302 {
sgrsn 0:08b48d8e6eab 303 writeSomeData(Arduino_Address, TARGET_X, vx, 4);
sgrsn 0:08b48d8e6eab 304 writeSomeData(Arduino_Address, TARGET_Y, vy, 4);
sgrsn 0:08b48d8e6eab 305 writeSomeData(Arduino_Address, TARGET_R, vo, 4);
sgrsn 0:08b48d8e6eab 306 }
sgrsn 0:08b48d8e6eab 307 }
sgrsn 0:08b48d8e6eab 308
sgrsn 0:08b48d8e6eab 309 else if(abs(Gcode[0]-x)<5 && abs(Gcode[1]-y)<5 && abs(Gcode[2]-angleZ)<2 && abs(vo)<2)
sgrsn 0:08b48d8e6eab 310 {
sgrsn 0:08b48d8e6eab 311 //車輪の停止(念のため5回)
sgrsn 0:08b48d8e6eab 312 for(int _ = 0; _ < 5; _++)
sgrsn 0:08b48d8e6eab 313 {
sgrsn 0:08b48d8e6eab 314 writeSomeData(Arduino_Address, TARGET_X, 0, 4);
sgrsn 0:08b48d8e6eab 315 wait_ms(1);
sgrsn 0:08b48d8e6eab 316 writeSomeData(Arduino_Address, TARGET_Y, 0, 4);
sgrsn 0:08b48d8e6eab 317 wait_ms(1);
sgrsn 0:08b48d8e6eab 318 writeSomeData(Arduino_Address, TARGET_R, 0, 4);
sgrsn 0:08b48d8e6eab 319 wait_ms(1);
sgrsn 0:08b48d8e6eab 320 }
sgrsn 0:08b48d8e6eab 321
sgrsn 0:08b48d8e6eab 322 //シーケンスを進める
sgrsn 0:08b48d8e6eab 323 seq++;
sgrsn 0:08b48d8e6eab 324 //Gコード取得
sgrsn 0:08b48d8e6eab 325 code.getGcode(seq, Gcode_size, Gcode);
sgrsn 0:08b48d8e6eab 326 //次の地点までのパスを作成
sgrsn 0:08b48d8e6eab 327 path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 328 //パスのリセット
sgrsn 0:08b48d8e6eab 329 path = 0;
sgrsn 0:08b48d8e6eab 330
sgrsn 0:08b48d8e6eab 331 //waitがあるのでジャイロのリセット
sgrsn 0:08b48d8e6eab 332 jy901.reset();
sgrsn 0:08b48d8e6eab 333 //一応pidもリセットしとく
sgrsn 0:08b48d8e6eab 334 pid_x.reset();
sgrsn 0:08b48d8e6eab 335 pid_y.reset();
sgrsn 0:08b48d8e6eab 336 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 337 }
sgrsn 0:08b48d8e6eab 338 else //目標地点に到達していないとき
sgrsn 0:08b48d8e6eab 339 {
sgrsn 0:08b48d8e6eab 340 writeSomeData(Arduino_Address, TARGET_X, vx, 4);
sgrsn 0:08b48d8e6eab 341 writeSomeData(Arduino_Address, TARGET_Y, vy, 4);
sgrsn 0:08b48d8e6eab 342 writeSomeData(Arduino_Address, TARGET_R, vo, 4);
sgrsn 0:08b48d8e6eab 343 }
sgrsn 0:08b48d8e6eab 344 LED = seq;
sgrsn 0:08b48d8e6eab 345 }
sgrsn 0:08b48d8e6eab 346 LED = 7;
sgrsn 0:08b48d8e6eab 347 motorStop();
sgrsn 0:08b48d8e6eab 348 wait_ms(500);
sgrsn 0:08b48d8e6eab 349 }
sgrsn 0:08b48d8e6eab 350 void followMe()
sgrsn 0:08b48d8e6eab 351 {
sgrsn 0:08b48d8e6eab 352 writeSomeData(Arduino_Address, TARGET_Z, 120, 4);
sgrsn 0:08b48d8e6eab 353 writeSomeData(Arduino_Address, SERVO_ENABLE, 1, 4);
sgrsn 0:08b48d8e6eab 354 wait_ms(1000);
sgrsn 0:08b48d8e6eab 355 pid_x.setParameter(1.0, 0, 0);//(6.0, 0.57851);
sgrsn 0:08b48d8e6eab 356 pid_y.setParameter(0.7, 0, 0);//(2.0, 0.7155);
sgrsn 0:08b48d8e6eab 357 pid_yaw.setParameter(7, 0.6579);
sgrsn 0:08b48d8e6eab 358
sgrsn 0:08b48d8e6eab 359 int path = 0;
sgrsn 0:08b48d8e6eab 360 int path_size = 1;
sgrsn 0:08b48d8e6eab 361
sgrsn 0:08b48d8e6eab 362 int modeAngle = 0;
sgrsn 0:08b48d8e6eab 363
sgrsn 0:08b48d8e6eab 364 //
sgrsn 0:08b48d8e6eab 365 int guideX = 0, guideY = 150;
sgrsn 0:08b48d8e6eab 366 int targetX = 0;
sgrsn 0:08b48d8e6eab 367 int targetY = 0;
sgrsn 0:08b48d8e6eab 368 float angleZ = 0;
sgrsn 0:08b48d8e6eab 369 float targetR = 0;
sgrsn 0:08b48d8e6eab 370 float x = 0, y = 0;
sgrsn 0:08b48d8e6eab 371 float vx = 0, vy = 0, vo = 0;
sgrsn 0:08b48d8e6eab 372
sgrsn 0:08b48d8e6eab 373 int followMode = 0;
sgrsn 0:08b48d8e6eab 374
sgrsn 0:08b48d8e6eab 375 pid_x.reset();
sgrsn 0:08b48d8e6eab 376 pid_y.reset();
sgrsn 0:08b48d8e6eab 377 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 378 while(1)
sgrsn 0:08b48d8e6eab 379 {
sgrsn 0:08b48d8e6eab 380 //自己位置推定
sgrsn 0:08b48d8e6eab 381 angleZ = jy901.calculateAngleOnlyGyro();
sgrsn 0:08b48d8e6eab 382 float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 383 float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 384 x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 385 y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 386
sgrsn 0:08b48d8e6eab 387 //ガイドロボット相対位置取得
sgrsn 0:08b48d8e6eab 388 guideX = Register[FRONT_OBJ_POSITION];
sgrsn 0:08b48d8e6eab 389 guideY = Register[FRONT_OBJ_DISTANCE];
sgrsn 0:08b48d8e6eab 390
sgrsn 0:08b48d8e6eab 391 if(y > 900 && followMode == 0)
sgrsn 0:08b48d8e6eab 392 {
sgrsn 0:08b48d8e6eab 393 followMode = 1;
sgrsn 0:08b48d8e6eab 394 }
sgrsn 0:08b48d8e6eab 395 if(followMode == 1)
sgrsn 0:08b48d8e6eab 396 {
sgrsn 0:08b48d8e6eab 397 pid_yaw.setParameter(1.5, 0, 0);
sgrsn 0:08b48d8e6eab 398 targetR = atan2(double(-guideX), double(guideY))*RAD_TO_DEG;
sgrsn 0:08b48d8e6eab 399 vx = 0;
sgrsn 0:08b48d8e6eab 400 vy = pid_y.controlPID(guideY, 150);
sgrsn 0:08b48d8e6eab 401 //vy = 0;
sgrsn 0:08b48d8e6eab 402 vo = pid_yaw.controlPID(targetR, 0);
sgrsn 0:08b48d8e6eab 403 if(angleZ > 40)
sgrsn 0:08b48d8e6eab 404 {
sgrsn 0:08b48d8e6eab 405 followMode = 2;
sgrsn 0:08b48d8e6eab 406 modeAngle = 90;
sgrsn 0:08b48d8e6eab 407 }
sgrsn 0:08b48d8e6eab 408 else if(angleZ < -40)
sgrsn 0:08b48d8e6eab 409 {
sgrsn 0:08b48d8e6eab 410 followMode = 2;
sgrsn 0:08b48d8e6eab 411 modeAngle = -90;
sgrsn 0:08b48d8e6eab 412 }
sgrsn 0:08b48d8e6eab 413 }
sgrsn 0:08b48d8e6eab 414 if(followMode == 2)
sgrsn 0:08b48d8e6eab 415 {
sgrsn 0:08b48d8e6eab 416 path_size = makePath(x, 1300, modeAngle, x, y, angleZ);
sgrsn 0:08b48d8e6eab 417 followMode = 3;
sgrsn 0:08b48d8e6eab 418 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 419 }
sgrsn 0:08b48d8e6eab 420 if(followMode == 3)
sgrsn 0:08b48d8e6eab 421 {
sgrsn 0:08b48d8e6eab 422 pid_yaw.setParameter(7, 0.6579);
sgrsn 0:08b48d8e6eab 423 //経路更新
sgrsn 0:08b48d8e6eab 424 path++;
sgrsn 0:08b48d8e6eab 425 if(path >= path_size)path = path_size-1;
sgrsn 0:08b48d8e6eab 426 targetX = PATH[path][0];
sgrsn 0:08b48d8e6eab 427 targetY = PATH[path][1];
sgrsn 0:08b48d8e6eab 428 targetR = PATH[path][2];
sgrsn 0:08b48d8e6eab 429 vx = pid_x.controlPID(targetX, x);
sgrsn 0:08b48d8e6eab 430 vy = pid_y.controlPID(targetY, y);
sgrsn 0:08b48d8e6eab 431 vo = pid_yaw.controlPID(targetR, angleZ);
sgrsn 0:08b48d8e6eab 432 if(y > 1280)
sgrsn 0:08b48d8e6eab 433 {
sgrsn 0:08b48d8e6eab 434 vx = 0;
sgrsn 0:08b48d8e6eab 435 vy = 0;
sgrsn 0:08b48d8e6eab 436 vo = 0;
sgrsn 0:08b48d8e6eab 437 followMode = 4;
sgrsn 0:08b48d8e6eab 438 }
sgrsn 0:08b48d8e6eab 439 }
sgrsn 0:08b48d8e6eab 440 if(followMode == 4 && x < -1200)
sgrsn 0:08b48d8e6eab 441 {
sgrsn 0:08b48d8e6eab 442 //followMode = 5;
sgrsn 0:08b48d8e6eab 443 }
sgrsn 0:08b48d8e6eab 444 if(followMode == 5)
sgrsn 0:08b48d8e6eab 445 {
sgrsn 0:08b48d8e6eab 446 pid_yaw.setParameter(1.5, 0, 0);
sgrsn 0:08b48d8e6eab 447 targetR = atan2(double(-guideX), double(guideY))*RAD_TO_DEG;
sgrsn 0:08b48d8e6eab 448 vx = 0;
sgrsn 0:08b48d8e6eab 449 //vy = pid_y.controlPID(targetY, 150);
sgrsn 0:08b48d8e6eab 450 vy = 0;
sgrsn 0:08b48d8e6eab 451 vo = pid_yaw.controlPID(targetR, 0);
sgrsn 0:08b48d8e6eab 452 if(angleZ > 130)
sgrsn 0:08b48d8e6eab 453 {
sgrsn 0:08b48d8e6eab 454 path_size = makePath(-1500, y, 180, x, y, angleZ);
sgrsn 0:08b48d8e6eab 455 path = 0;
sgrsn 0:08b48d8e6eab 456 followMode = 6;
sgrsn 0:08b48d8e6eab 457 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 458 }
sgrsn 0:08b48d8e6eab 459 }
sgrsn 0:08b48d8e6eab 460 if(followMode == 6)
sgrsn 0:08b48d8e6eab 461 {
sgrsn 0:08b48d8e6eab 462 pid_yaw.setParameter(7, 0.6579);
sgrsn 0:08b48d8e6eab 463 //経路更新
sgrsn 0:08b48d8e6eab 464 path++;
sgrsn 0:08b48d8e6eab 465 if(path >= path_size)path = path_size-1;
sgrsn 0:08b48d8e6eab 466 targetX = PATH[path][0];
sgrsn 0:08b48d8e6eab 467 targetY = PATH[path][1];
sgrsn 0:08b48d8e6eab 468 targetR = PATH[path][2];
sgrsn 0:08b48d8e6eab 469 vx = pid_x.controlPID(targetX, x);
sgrsn 0:08b48d8e6eab 470 vy = pid_y.controlPID(targetY, y);
sgrsn 0:08b48d8e6eab 471 vo = pid_yaw.controlPID(targetR, angleZ);
sgrsn 0:08b48d8e6eab 472 if(x < -1450)
sgrsn 0:08b48d8e6eab 473 {
sgrsn 0:08b48d8e6eab 474 vx = 0;
sgrsn 0:08b48d8e6eab 475 vy = 0;
sgrsn 0:08b48d8e6eab 476 vo = 0;
sgrsn 0:08b48d8e6eab 477 followMode = 7;
sgrsn 0:08b48d8e6eab 478 }
sgrsn 0:08b48d8e6eab 479 }
sgrsn 0:08b48d8e6eab 480 if(guideY < 1)
sgrsn 0:08b48d8e6eab 481 {
sgrsn 0:08b48d8e6eab 482 vx = 0;
sgrsn 0:08b48d8e6eab 483 vy = 0;
sgrsn 0:08b48d8e6eab 484 }
sgrsn 0:08b48d8e6eab 485 else if(followMode == 0 || followMode == 4 || followMode == 7)
sgrsn 0:08b48d8e6eab 486 {
sgrsn 0:08b48d8e6eab 487 //目標速度
sgrsn 0:08b48d8e6eab 488 vx = pid_x.controlPID(guideX, 0);
sgrsn 0:08b48d8e6eab 489 vy = pid_y.controlPID(guideY, 150);
sgrsn 0:08b48d8e6eab 490 vo = pid_yaw.controlPID(targetR, angleZ);
sgrsn 0:08b48d8e6eab 491 }
sgrsn 0:08b48d8e6eab 492
sgrsn 0:08b48d8e6eab 493 writeSomeData(Arduino_Address, TARGET_X, vx, 4);
sgrsn 0:08b48d8e6eab 494 writeSomeData(Arduino_Address, TARGET_Y, vy, 4);
sgrsn 0:08b48d8e6eab 495 writeSomeData(Arduino_Address, TARGET_R, vo, 4);
sgrsn 0:08b48d8e6eab 496 //wait_ms(30);
sgrsn 0:08b48d8e6eab 497 }
sgrsn 0:08b48d8e6eab 498 }
sgrsn 0:08b48d8e6eab 499
sgrsn 0:08b48d8e6eab 500 void pickAndPlace(FILE *fp)
sgrsn 0:08b48d8e6eab 501 {
sgrsn 0:08b48d8e6eab 502 MakeSequencer code(fp);
sgrsn 0:08b48d8e6eab 503 const int seq_size = code.getGcodeSize();
sgrsn 0:08b48d8e6eab 504 int Gcode[Gcode_size] = {};
sgrsn 0:08b48d8e6eab 505 code.getGcode(0, Gcode_size, Gcode);
sgrsn 0:08b48d8e6eab 506 for(int i = 0; i < Gcode_size; i++)
sgrsn 0:08b48d8e6eab 507 printf("%d, ", Gcode[i]);
sgrsn 0:08b48d8e6eab 508 printf("\r\n");
sgrsn 0:08b48d8e6eab 509
sgrsn 0:08b48d8e6eab 510 pid_x.setParameter(1.5, 0.57851);//(6.0, 0.57851);
sgrsn 0:08b48d8e6eab 511 pid_y.setParameter(2.0, 0.58);//(2.0, 0.7155);
sgrsn 0:08b48d8e6eab 512 pid_yaw.setParameter(7, 0.6579);
sgrsn 0:08b48d8e6eab 513
sgrsn 0:08b48d8e6eab 514 float angleZ = 0;
sgrsn 0:08b48d8e6eab 515 float x = 0, y = 0;
sgrsn 0:08b48d8e6eab 516
sgrsn 0:08b48d8e6eab 517 int linePos = line.getPosition();
sgrsn 0:08b48d8e6eab 518 int last_linePos = linePos;
sgrsn 0:08b48d8e6eab 519 int d_linePos = 0;
sgrsn 0:08b48d8e6eab 520
sgrsn 0:08b48d8e6eab 521 int seq = 0;
sgrsn 0:08b48d8e6eab 522 int path = 0;
sgrsn 0:08b48d8e6eab 523
sgrsn 0:08b48d8e6eab 524 int targetZ = 0;
sgrsn 0:08b48d8e6eab 525 int hand = 10;
sgrsn 0:08b48d8e6eab 526 int last_targetZ = 0;
sgrsn 0:08b48d8e6eab 527 int last_hand = 10;
sgrsn 0:08b48d8e6eab 528
sgrsn 0:08b48d8e6eab 529 int controlFlag = 1;
sgrsn 0:08b48d8e6eab 530
sgrsn 0:08b48d8e6eab 531 int path_size = makePath(Gcode[seq], Gcode[seq], Gcode[seq], x, y, angleZ);
sgrsn 0:08b48d8e6eab 532
sgrsn 0:08b48d8e6eab 533 jy901.reset();
sgrsn 0:08b48d8e6eab 534 pid_x.reset();
sgrsn 0:08b48d8e6eab 535 pid_y.reset();
sgrsn 0:08b48d8e6eab 536 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 537
sgrsn 0:08b48d8e6eab 538 float last_t_lineRead = 0;
sgrsn 0:08b48d8e6eab 539
sgrsn 0:08b48d8e6eab 540 while(seq < seq_size)
sgrsn 0:08b48d8e6eab 541 {
sgrsn 0:08b48d8e6eab 542 //自己位置推定
sgrsn 0:08b48d8e6eab 543 angleZ = jy901.calculateAngleOnlyGyro();
sgrsn 0:08b48d8e6eab 544 float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 545 float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r;
sgrsn 0:08b48d8e6eab 546 x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 547 y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 548
sgrsn 0:08b48d8e6eab 549 //printf("%f, %f\r\n", x, y);
sgrsn 0:08b48d8e6eab 550
sgrsn 0:08b48d8e6eab 551 if(timer2.read() - last_t_lineRead > 0.100)
sgrsn 0:08b48d8e6eab 552 {
sgrsn 0:08b48d8e6eab 553 last_linePos = linePos;
sgrsn 0:08b48d8e6eab 554 linePos = line.getPosition();
sgrsn 0:08b48d8e6eab 555 d_linePos = linePos - last_linePos;
sgrsn 0:08b48d8e6eab 556 last_t_lineRead = timer2.read();
sgrsn 0:08b48d8e6eab 557 }
sgrsn 0:08b48d8e6eab 558
sgrsn 0:08b48d8e6eab 559 //目標位置取得
sgrsn 0:08b48d8e6eab 560 int targetX = PATH[path][0];
sgrsn 0:08b48d8e6eab 561 int targetY = PATH[path][1];
sgrsn 0:08b48d8e6eab 562 int targetR = PATH[path][2];
sgrsn 0:08b48d8e6eab 563 //経路更新
sgrsn 0:08b48d8e6eab 564 path++;
sgrsn 0:08b48d8e6eab 565 if(path >= path_size)path = path_size-1;
sgrsn 0:08b48d8e6eab 566
sgrsn 0:08b48d8e6eab 567 //目標速度ベクトル計算
sgrsn 0:08b48d8e6eab 568 float xd = targetX - x;
sgrsn 0:08b48d8e6eab 569 float yd = targetY - y;
sgrsn 0:08b48d8e6eab 570 float r = sqrt(xd*xd + yd*yd);
sgrsn 0:08b48d8e6eab 571 float fai = atan2(yd, xd) - angleZ*DEG_TO_RAD;
sgrsn 0:08b48d8e6eab 572 float target_dx = r*cos(fai);
sgrsn 0:08b48d8e6eab 573 float target_dy = r*sin(fai);
sgrsn 0:08b48d8e6eab 574 float vx = pid_x.controlPID(target_dx, 0);
sgrsn 0:08b48d8e6eab 575 float vy = pid_y.controlPID(target_dy, 0);
sgrsn 0:08b48d8e6eab 576 float vo = pid_yaw.controlPID(targetR, angleZ);
sgrsn 0:08b48d8e6eab 577
sgrsn 0:08b48d8e6eab 578 //printf("%f, %f, %f\r\n", x, y, angleZ);
sgrsn 0:08b48d8e6eab 579 //printf("%f:%d, %f:%d, %f:%d\r\n", x, targetX, y, targetY, angleZ, targetR);
sgrsn 0:08b48d8e6eab 580 //printf("%d, %d, %d\r\n", targetX, targetY, targetR);
sgrsn 0:08b48d8e6eab 581 //printf("%f, %f, %f\r\n", vx, vy, vo);
sgrsn 0:08b48d8e6eab 582
sgrsn 0:08b48d8e6eab 583 //目標地点に到達したときの処理
sgrsn 0:08b48d8e6eab 584 //if(abs(Gcode[seq][0]-x)<5 && abs(Gcode[seq][1]-y)<5 && abs(Gcode[seq][2]-angleZ)<3)
sgrsn 0:08b48d8e6eab 585 if(abs(Gcode[0]-x)<5 && abs(Gcode[1]-y)<5 && abs(Gcode[2]-angleZ)<10)
sgrsn 0:08b48d8e6eab 586 {
sgrsn 0:08b48d8e6eab 587 //車輪の停止(念のため5回)
sgrsn 0:08b48d8e6eab 588 for(int _ = 0; _ < 1; _++)
sgrsn 0:08b48d8e6eab 589 {
sgrsn 0:08b48d8e6eab 590 writeSomeData(Arduino_Address, TARGET_X, 0, 4);
sgrsn 0:08b48d8e6eab 591 wait_ms(1);
sgrsn 0:08b48d8e6eab 592 writeSomeData(Arduino_Address, TARGET_Y, 0, 4);
sgrsn 0:08b48d8e6eab 593 wait_ms(1);
sgrsn 0:08b48d8e6eab 594 writeSomeData(Arduino_Address, TARGET_R, 0, 4);
sgrsn 0:08b48d8e6eab 595 wait_ms(1);
sgrsn 0:08b48d8e6eab 596 }
sgrsn 0:08b48d8e6eab 597
sgrsn 0:08b48d8e6eab 598 last_targetZ = targetZ;
sgrsn 0:08b48d8e6eab 599 last_hand = hand;
sgrsn 0:08b48d8e6eab 600 targetZ = Gcode[3];
sgrsn 0:08b48d8e6eab 601 hand = Gcode[4];
sgrsn 0:08b48d8e6eab 602
sgrsn 0:08b48d8e6eab 603 if(Gcode[5] == 1)
sgrsn 0:08b48d8e6eab 604 {
sgrsn 0:08b48d8e6eab 605 alarm(3);
sgrsn 0:08b48d8e6eab 606 }
sgrsn 0:08b48d8e6eab 607 if(targetZ != last_targetZ)
sgrsn 0:08b48d8e6eab 608 {
sgrsn 0:08b48d8e6eab 609 printf("%d\r\n", targetZ);
sgrsn 0:08b48d8e6eab 610 writeSomeData(Arduino_Address, TARGET_Z, targetZ, 4);
sgrsn 0:08b48d8e6eab 611 writeSomeData(Arduino_Address, SERVO_ENABLE, 1, 4);
sgrsn 0:08b48d8e6eab 612 controlFlag = 0;
sgrsn 0:08b48d8e6eab 613 wait_ms(abs(targetZ-last_targetZ)*20);
sgrsn 0:08b48d8e6eab 614 }
sgrsn 0:08b48d8e6eab 615 if(hand != last_hand)
sgrsn 0:08b48d8e6eab 616 {
sgrsn 0:08b48d8e6eab 617 writeSomeData(Arduino_Address, TARGET_H, Gcode[4], 4);
sgrsn 0:08b48d8e6eab 618 controlFlag = 0;
sgrsn 0:08b48d8e6eab 619 wait_ms(1000);
sgrsn 0:08b48d8e6eab 620 }
sgrsn 0:08b48d8e6eab 621
sgrsn 0:08b48d8e6eab 622 //シーケンスを進める
sgrsn 0:08b48d8e6eab 623 seq++;
sgrsn 0:08b48d8e6eab 624 //Gコード取得
sgrsn 0:08b48d8e6eab 625 code.getGcode(seq, Gcode_size, Gcode);
sgrsn 0:08b48d8e6eab 626 for(int i = 0; i < Gcode_size; i++)
sgrsn 0:08b48d8e6eab 627 printf("%d, ", Gcode[i]);
sgrsn 0:08b48d8e6eab 628 printf("\r\n");
sgrsn 0:08b48d8e6eab 629 //次の地点までのパスを作成
sgrsn 0:08b48d8e6eab 630 path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ);
sgrsn 0:08b48d8e6eab 631 //パスのリセット
sgrsn 0:08b48d8e6eab 632 path = 0;
sgrsn 0:08b48d8e6eab 633
sgrsn 0:08b48d8e6eab 634 //waitがあるのでジャイロのリセット
sgrsn 0:08b48d8e6eab 635 jy901.reset();
sgrsn 0:08b48d8e6eab 636 //一応pidもリセットしとく
sgrsn 0:08b48d8e6eab 637 pid_x.reset();
sgrsn 0:08b48d8e6eab 638 pid_y.reset();
sgrsn 0:08b48d8e6eab 639 pid_yaw.reset();
sgrsn 0:08b48d8e6eab 640 }
sgrsn 0:08b48d8e6eab 641 else //目標地点に到達していないとき
sgrsn 0:08b48d8e6eab 642 {
sgrsn 0:08b48d8e6eab 643 writeSomeData(Arduino_Address, TARGET_X, vx, 4);
sgrsn 0:08b48d8e6eab 644 writeSomeData(Arduino_Address, TARGET_Y, vy, 4);
sgrsn 0:08b48d8e6eab 645 writeSomeData(Arduino_Address, TARGET_R, vo, 4);
sgrsn 0:08b48d8e6eab 646 }
sgrsn 0:08b48d8e6eab 647 }
sgrsn 0:08b48d8e6eab 648 }
sgrsn 0:08b48d8e6eab 649
sgrsn 0:08b48d8e6eab 650 void xbeeControll()
sgrsn 0:08b48d8e6eab 651 {
sgrsn 0:08b48d8e6eab 652 PacketSerial xbee(USBTX, USBRX, Register, 115200);
sgrsn 0:08b48d8e6eab 653
sgrsn 0:08b48d8e6eab 654 while(1)
sgrsn 0:08b48d8e6eab 655 {
sgrsn 0:08b48d8e6eab 656 }
sgrsn 0:08b48d8e6eab 657 }
sgrsn 0:08b48d8e6eab 658
sgrsn 0:08b48d8e6eab 659 enum TASK
sgrsn 0:08b48d8e6eab 660 {
sgrsn 0:08b48d8e6eab 661 ROBOT_GUIDE = 6, //110
sgrsn 0:08b48d8e6eab 662 FOLLOW_ME = 5, //101
sgrsn 0:08b48d8e6eab 663 PICK_AND_PLACE = 3, //011
sgrsn 0:08b48d8e6eab 664 CONTROLL = 7 //111
sgrsn 0:08b48d8e6eab 665 };
sgrsn 0:08b48d8e6eab 666
sgrsn 0:08b48d8e6eab 667 int main()
sgrsn 0:08b48d8e6eab 668 {
sgrsn 0:08b48d8e6eab 669 for(int _ = 0; _ < 5; _++)
sgrsn 0:08b48d8e6eab 670 {
sgrsn 0:08b48d8e6eab 671 writeSomeData(Arduino_Address, TARGET_X, 0, 4);
sgrsn 0:08b48d8e6eab 672 writeSomeData(Arduino_Address, TARGET_Y, 0, 4);
sgrsn 0:08b48d8e6eab 673 writeSomeData(Arduino_Address, TARGET_R, 0, 4);
sgrsn 0:08b48d8e6eab 674 }
sgrsn 0:08b48d8e6eab 675 LED = 0;
sgrsn 0:08b48d8e6eab 676 buzzer = 0;
sgrsn 0:08b48d8e6eab 677
sgrsn 0:08b48d8e6eab 678 line.setBarStrobe();
sgrsn 0:08b48d8e6eab 679 line.clearInvertBits();
sgrsn 0:08b48d8e6eab 680 uint8_t returnStatus = line.begin();
sgrsn 0:08b48d8e6eab 681 jy901.calibrateAll(5000);
sgrsn 0:08b48d8e6eab 682
sgrsn 0:08b48d8e6eab 683 bool buttonFlag = 0;
sgrsn 0:08b48d8e6eab 684 int buttonTime = 0;
sgrsn 0:08b48d8e6eab 685 int mode = 1;
sgrsn 0:08b48d8e6eab 686 int task = 0;
sgrsn 0:08b48d8e6eab 687 alarm();
sgrsn 0:08b48d8e6eab 688 while(1)
sgrsn 0:08b48d8e6eab 689 {
sgrsn 0:08b48d8e6eab 690 LED = mode;
sgrsn 0:08b48d8e6eab 691 task = modeChange.read();
sgrsn 0:08b48d8e6eab 692 //押してないとき
sgrsn 0:08b48d8e6eab 693 if(button == 1)
sgrsn 0:08b48d8e6eab 694 {
sgrsn 0:08b48d8e6eab 695 if(timer.read()-buttonTime < 1.0 && buttonFlag == 0)
sgrsn 0:08b48d8e6eab 696 {
sgrsn 0:08b48d8e6eab 697 mode++;
sgrsn 0:08b48d8e6eab 698 if(mode >= 5)mode = 0;
sgrsn 0:08b48d8e6eab 699 wait_ms(300);
sgrsn 0:08b48d8e6eab 700 }
sgrsn 0:08b48d8e6eab 701 buttonFlag = 1;
sgrsn 0:08b48d8e6eab 702
sgrsn 0:08b48d8e6eab 703 }
sgrsn 0:08b48d8e6eab 704 //押したとき
sgrsn 0:08b48d8e6eab 705 else
sgrsn 0:08b48d8e6eab 706 {
sgrsn 0:08b48d8e6eab 707 if(buttonFlag == 1)
sgrsn 0:08b48d8e6eab 708 {
sgrsn 0:08b48d8e6eab 709 buttonTime = timer.read();
sgrsn 0:08b48d8e6eab 710 }
sgrsn 0:08b48d8e6eab 711 if(timer.read()-buttonTime > 1.5)
sgrsn 0:08b48d8e6eab 712 {
sgrsn 0:08b48d8e6eab 713 //実行処理
sgrsn 0:08b48d8e6eab 714 LED = 0;
sgrsn 0:08b48d8e6eab 715 switch(task)
sgrsn 0:08b48d8e6eab 716 {
sgrsn 0:08b48d8e6eab 717 case CONTROLL:
sgrsn 0:08b48d8e6eab 718 FILE *demofp = fopen( "/local/demo.txt", "r");
sgrsn 0:08b48d8e6eab 719 pickAndPlace(demofp);
sgrsn 0:08b48d8e6eab 720 fclose(demofp);
sgrsn 0:08b48d8e6eab 721 break;
sgrsn 0:08b48d8e6eab 722 case ROBOT_GUIDE:
sgrsn 0:08b48d8e6eab 723 FILE *robotfp;
sgrsn 0:08b48d8e6eab 724 switch(mode)
sgrsn 0:08b48d8e6eab 725 {
sgrsn 0:08b48d8e6eab 726 case 1:
sgrsn 0:08b48d8e6eab 727 robotfp = fopen( "/local/robot1.txt", "r");
sgrsn 0:08b48d8e6eab 728 break;
sgrsn 0:08b48d8e6eab 729 case 2:
sgrsn 0:08b48d8e6eab 730 robotfp = fopen( "/local/robot2.txt", "r");
sgrsn 0:08b48d8e6eab 731 break;
sgrsn 0:08b48d8e6eab 732 case 3:
sgrsn 0:08b48d8e6eab 733 robotfp = fopen( "/local/robot3.txt", "r");
sgrsn 0:08b48d8e6eab 734 break;
sgrsn 0:08b48d8e6eab 735 case 4:
sgrsn 0:08b48d8e6eab 736 robotfp = fopen( "/local/robot4.txt", "r");
sgrsn 0:08b48d8e6eab 737 break;
sgrsn 0:08b48d8e6eab 738 }
sgrsn 0:08b48d8e6eab 739 robotGuide(robotfp);
sgrsn 0:08b48d8e6eab 740 break;
sgrsn 0:08b48d8e6eab 741 case FOLLOW_ME:
sgrsn 0:08b48d8e6eab 742 followMe();
sgrsn 0:08b48d8e6eab 743 break;
sgrsn 0:08b48d8e6eab 744 case PICK_AND_PLACE:
sgrsn 0:08b48d8e6eab 745
sgrsn 0:08b48d8e6eab 746 FILE *fp;
sgrsn 0:08b48d8e6eab 747 switch(mode)
sgrsn 0:08b48d8e6eab 748 {
sgrsn 0:08b48d8e6eab 749 case 1:
sgrsn 0:08b48d8e6eab 750 fp = fopen( "/local/guide1.txt", "r");
sgrsn 0:08b48d8e6eab 751 break;
sgrsn 0:08b48d8e6eab 752 case 2:
sgrsn 0:08b48d8e6eab 753 fp = fopen( "/local/guide2.txt", "r");
sgrsn 0:08b48d8e6eab 754 break;
sgrsn 0:08b48d8e6eab 755 case 3:
sgrsn 0:08b48d8e6eab 756 fp = fopen( "/local/guide3.txt", "r");
sgrsn 0:08b48d8e6eab 757 break;
sgrsn 0:08b48d8e6eab 758 case 4:
sgrsn 0:08b48d8e6eab 759 fp = fopen( "/local/guide4.txt", "r");
sgrsn 0:08b48d8e6eab 760 break;
sgrsn 0:08b48d8e6eab 761
sgrsn 0:08b48d8e6eab 762 default:
sgrsn 0:08b48d8e6eab 763 fp = fopen( "/local/guide1.txt", "r");
sgrsn 0:08b48d8e6eab 764 break;
sgrsn 0:08b48d8e6eab 765 }
sgrsn 0:08b48d8e6eab 766 pickAndPlace(fp);
sgrsn 0:08b48d8e6eab 767 break;
sgrsn 0:08b48d8e6eab 768
sgrsn 0:08b48d8e6eab 769 default:
sgrsn 0:08b48d8e6eab 770 alarm(2);
sgrsn 0:08b48d8e6eab 771 break;
sgrsn 0:08b48d8e6eab 772 }
sgrsn 0:08b48d8e6eab 773 }
sgrsn 0:08b48d8e6eab 774 buttonFlag = 0;
sgrsn 0:08b48d8e6eab 775 }
sgrsn 0:08b48d8e6eab 776 }
sgrsn 0:08b48d8e6eab 777 }
sgrsn 0:08b48d8e6eab 778
sgrsn 0:08b48d8e6eab 779 bool writeSomeData(char addr, char reg, int32_t data ,int size)
sgrsn 0:08b48d8e6eab 780 {
sgrsn 0:08b48d8e6eab 781 char tmp[2+size];
sgrsn 0:08b48d8e6eab 782 tmp[0] = reg;
sgrsn 0:08b48d8e6eab 783 tmp[1] = size;
sgrsn 0:08b48d8e6eab 784 for(int i = 0; i < size; i++)
sgrsn 0:08b48d8e6eab 785 {
sgrsn 0:08b48d8e6eab 786 tmp[i+2] = (data >> (i*8)) & 0xFF;
sgrsn 0:08b48d8e6eab 787 }
sgrsn 0:08b48d8e6eab 788 //char DATA[2] = {reg,size};
sgrsn 0:08b48d8e6eab 789 bool N = i2c.write(addr, tmp, 2+size);
sgrsn 0:08b48d8e6eab 790 //N|= i2c.write(addr, tmp, size);
sgrsn 0:08b48d8e6eab 791 return N;
sgrsn 0:08b48d8e6eab 792 }
sgrsn 0:08b48d8e6eab 793
sgrsn 0:08b48d8e6eab 794 int makePath(int targetX, int targetY, int targetR, int x, int y, int angleZ)
sgrsn 0:08b48d8e6eab 795 {
sgrsn 0:08b48d8e6eab 796 int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetR-angleZ) ) / 1.0; //1mm間隔
sgrsn 0:08b48d8e6eab 797 //printf("num = %d\r\n", num);
sgrsn 0:08b48d8e6eab 798 for(int i = 1; i <= num; i++)
sgrsn 0:08b48d8e6eab 799 {
sgrsn 0:08b48d8e6eab 800 float a = float(i) / num;
sgrsn 0:08b48d8e6eab 801 PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 802 PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD);
sgrsn 0:08b48d8e6eab 803 PATH[i-1][2] = angleZ + float(targetR - angleZ) * a;
sgrsn 0:08b48d8e6eab 804 }
sgrsn 0:08b48d8e6eab 805 if(num > 0)
sgrsn 0:08b48d8e6eab 806 {
sgrsn 0:08b48d8e6eab 807 PATH[num-1][0] = targetX;
sgrsn 0:08b48d8e6eab 808 PATH[num-1][1] = targetY;
sgrsn 0:08b48d8e6eab 809 PATH[num-1][2] = targetR;
sgrsn 0:08b48d8e6eab 810 }
sgrsn 0:08b48d8e6eab 811 else
sgrsn 0:08b48d8e6eab 812 {
sgrsn 0:08b48d8e6eab 813 PATH[0][0] = targetX;
sgrsn 0:08b48d8e6eab 814 PATH[0][1] = targetY;
sgrsn 0:08b48d8e6eab 815 PATH[0][2] = targetR;
sgrsn 0:08b48d8e6eab 816 num = 1;
sgrsn 0:08b48d8e6eab 817 }
sgrsn 0:08b48d8e6eab 818 return num;
sgrsn 0:08b48d8e6eab 819 }
sgrsn 0:08b48d8e6eab 820
sgrsn 0:08b48d8e6eab 821 void alarm(int n)
sgrsn 0:08b48d8e6eab 822 {
sgrsn 0:08b48d8e6eab 823 int frequency[] = {NOTE_G5, NOTE_C5, NOTE_E5, NOTE_AS5, NOTE_DS5};//, 1, NOTE_GS5, NOTE_CS5, NOTE_F5, NOTE_B5, NOTE_E5, 1};
sgrsn 0:08b48d8e6eab 824 int beat[] = {16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16};
sgrsn 0:08b48d8e6eab 825 for (int i = 0; i < n; i++)
sgrsn 0:08b48d8e6eab 826 {
sgrsn 0:08b48d8e6eab 827 buzzer.period(1.0 / frequency[i]);
sgrsn 0:08b48d8e6eab 828 buzzer.write(0.5);
sgrsn 0:08b48d8e6eab 829 wait(1.0 / beat[i]);
sgrsn 0:08b48d8e6eab 830 buzzer.write(0);
sgrsn 0:08b48d8e6eab 831 wait(0.05);
sgrsn 0:08b48d8e6eab 832 }
sgrsn 0:08b48d8e6eab 833 buzzer = 0;
sgrsn 0:08b48d8e6eab 834 }
sgrsn 0:08b48d8e6eab 835 void motorStop()
sgrsn 0:08b48d8e6eab 836 {
sgrsn 0:08b48d8e6eab 837 for(int _ = 0; _ < 5; _++)
sgrsn 0:08b48d8e6eab 838 {
sgrsn 0:08b48d8e6eab 839 writeSomeData(Arduino_Address, TARGET_X, 0, 4);
sgrsn 0:08b48d8e6eab 840 wait_ms(1);
sgrsn 0:08b48d8e6eab 841 writeSomeData(Arduino_Address, TARGET_Y, 0, 4);
sgrsn 0:08b48d8e6eab 842 wait_ms(1);
sgrsn 0:08b48d8e6eab 843 writeSomeData(Arduino_Address, TARGET_R, 0, 4);
sgrsn 0:08b48d8e6eab 844 wait_ms(1);
sgrsn 0:08b48d8e6eab 845 }
sgrsn 0:08b48d8e6eab 846 }
sgrsn 0:08b48d8e6eab 847
sgrsn 0:08b48d8e6eab 848 float sign(float val)
sgrsn 0:08b48d8e6eab 849 {
sgrsn 0:08b48d8e6eab 850 if(val > 0)
sgrsn 0:08b48d8e6eab 851 {
sgrsn 0:08b48d8e6eab 852 return 1.0;
sgrsn 0:08b48d8e6eab 853 }
sgrsn 0:08b48d8e6eab 854 else if(val < 0)
sgrsn 0:08b48d8e6eab 855 {
sgrsn 0:08b48d8e6eab 856 return -1.0;
sgrsn 0:08b48d8e6eab 857 }
sgrsn 0:08b48d8e6eab 858 else
sgrsn 0:08b48d8e6eab 859 {
sgrsn 0:08b48d8e6eab 860 return 0;
sgrsn 0:08b48d8e6eab 861 }
sgrsn 0:08b48d8e6eab 862 }