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?
User | Revision | Line number | New 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 | } |