![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For our Robot
Dependencies: JY901 Make_Sequencer_3 PID PacketSerial QEI_simple SensorBar mbed
Diff: main.cpp
- Revision:
- 0:08b48d8e6eab
diff -r 000000000000 -r 08b48d8e6eab main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Nov 14 02:23:08 2018 +0000 @@ -0,0 +1,862 @@ +#include "mbed.h" +#include "JY901.h" +#include "QEI.h" +#include "PID.h" +#include "SensorBar.h" +#include "pitches.h" +#include "PacketSerial.h" +#include "MakeSequencer.h" + +// IIC Address +#define Arduino_Address 0x20<<1 +#define SX1509_ADDRESS 0x3E<<1 + +// Arduino Register Map +#define TARGET_X 0x10 +#define TARGET_Y 0x11 +#define TARGET_Z 0x12 +#define TARGET_R 0x13 +#define TARGET_H 0x14 +#define SERVO_ENABLE 0x20 +#define SERVO_DELAY 0x21 + +//RaspberryPi Register Map +#define SENSOR_ENABLE 0x00 +#define FRONT_OBJ_DISTANCE 0x01 +#define FRONT_OBJ_POSITION 0x02 +#define BACK_DISTANCE1 0x03 +#define BACK_DISTANCE2 0x04 +#define RIGHT_DISTANCE1 0x05 +#define RIGHT_DISTANCE2 0x06 +#define LEFT_DISTANCE1 0x07 +#define LEFT_DISTANCE2 0x08 + + +float pi = 3.14159265359; +float wheel_r = 38.0/2.0; //接地エンコーダのオムニ +float wheel_x_g = 50.0; //x軸接地エンコーダの重心からのずれ +float DEG_TO_RAD = pi / 180; +float RAD_TO_DEG = 180.0 / pi; + +const int Gcode_size = 6; //gコードの種類の数 + +I2C i2c(p28, p27); +Timer timer; +Timer timer2; +QEI RE[] = { QEI(p26, p25, 100, &timer), QEI(p23, p24, 100, &timer) }; +SensorBar line(&i2c, SX1509_ADDRESS); +PwmOut buzzer(p21); +DigitalIn button(p12); +BusIn modeChange(p15, p16, p17); +BusOut LED(p18, p19, p20); +LocalFileSystem local("local"); + +int32_t Register[10] = {}; +PacketSerial Raspi(USBTX, USBRX, Register, 115200); + +PID pid_x(&timer); +PID pid_y(&timer); +PID pid_yaw(&timer); +JY901 jy901(&i2c, &timer2); + +bool writeSomeData(char addr, char reg, int32_t data ,int size); + +int PATH[1500][3] = {}; +//int *PATH = new int[1000][3]; +int makePath(int targetX, int targetY, int targetR, int x, int y, int angleZ); + +void alarm(int n = 5); +void motorStop(); +float sign(float val); + +void robotGuide(FILE *fp) +{ + MakeSequencer code(fp); + const int seq_size = code.getGcodeSize(); + int Gcode[Gcode_size] = {}; + int awayCode[Gcode_size] = {}; + int awayFlag = 0; //0:awayなし, 1:awayあり, 2,前方に物体なし, 3:通過 + int objectSide = 0; + int laserSW = 0; + int laserSWFlag[2] = {}; + int deltaX = 0, deltaY = 0; + code.getGcode(0, Gcode_size, Gcode); + pid_x.setParameter(2.0, 0.57851);//(6.0, 0.57851); + pid_y.setParameter(2.0, 0.58);//(2.0, 0.7155); + pid_yaw.setParameter(7, 0.6579); + float x = 0, y = 0, angleZ = 0; + int objectX = 0, objectY = 2000; + int seq = 0; + int path = 0; + int path_size = makePath(Gcode[seq], Gcode[seq], Gcode[2], x, y, angleZ); + + float vx = 0, vy = 0, vo = 0; + + while(seq < seq_size) + { + //自己位置推定 + angleZ = jy901.calculateAngleOnlyGyro(); + float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r; + float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r; + x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD); + y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD); + + //障害物相対位置取得 + objectX = Register[FRONT_OBJ_POSITION]; + objectY = Register[FRONT_OBJ_DISTANCE]; + + //壁除外処理 + if( (y > 1000/* && angleZ < 10 && angleZ > -10*/) || (y < 500 && angleZ > 170 && angleZ < 190) ) + { + } + + //障害物発見 + else if(objectY < 250) + { + if(awayFlag != 1) + { + motorStop(); + wait_ms(500); + alarm(); + //waitがあるのでジャイロのリセット + jy901.reset(); + //一応pidもリセットしとく + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + //経路の再作成 + //左側 + if(objectX*cos(angleZ*DEG_TO_RAD) > 0 && angleZ < 10 && angleZ > -10) deltaX = -100, objectSide = 3; + //右側 + else if(objectX*cos(angleZ*DEG_TO_RAD) <= 0 && angleZ < 10 && angleZ > -10) deltaX = 100, objectSide = 2; + awayCode[0] = x + deltaX; + awayCode[1] = y; //yは維持 + awayCode[2] = angleZ; //角度も維持 + awayFlag = 1; + path_size = makePath(awayCode[0], awayCode[1], Gcode[2], x, y, angleZ); + path = 0; + } + } + else if(awayFlag == 2) + { + deltaX = 0; + //正方向 + if((Gcode[1]-y)*cos(angleZ*DEG_TO_RAD) > 0) deltaY = 100; + //負方向 + if((Gcode[1]-y)*cos(angleZ*DEG_TO_RAD) < 0) deltaY = -100; + awayCode[0] = x; + awayCode[1] = Gcode[1];//y+deltaY; + awayCode[2] = angleZ; //角度も維持 + path_size = makePath(awayCode[0], awayCode[1], Gcode[2], x, y, angleZ); + path = 0; + awayFlag = 3; + } + //左側 + if(objectSide == 2) + { + if(Register[LEFT_DISTANCE1] < 200) + { + if(laserSWFlag[0] == 1) + { + laserSW++; + laserSWFlag[0] = 0; + } + } + else + { + laserSWFlag[0] = 1; + } + if(Register[LEFT_DISTANCE2] < 200) + { + if(laserSWFlag[1] == 1) + { + laserSW++; + laserSWFlag[1] = 0; + } + } + else + { + laserSWFlag[1] = 1; + } + } + if(objectSide == 3) + { + if(Register[RIGHT_DISTANCE1] < 200) + { + if(laserSWFlag[0] == 1) + { + laserSW++; + laserSWFlag[0] = 0; + } + } + else + { + laserSWFlag[0] = 1; + } + if(Register[RIGHT_DISTANCE2] < 200) + { + if(laserSWFlag[1] == 1) + { + laserSW++; + laserSWFlag[1] = 0; + } + } + else + { + laserSWFlag[1] = 1; + } + } + + /*if(laserSW < 2 && awayFlag == 3) + { + awayFlag = 2; + }*/ + /*if(laserSW >= 2 && awayFlag == 3) + { + laserSW = 0; + awayCode[0] = Gcode[0]; + awayCode[1] = Gcode[1]; //yは維持 + awayCode[2] = angleZ; //角度も維持 + awayFlag = 4; + path_size = makePath(awayCode[0], awayCode[1], awayCode[2], x, y, angleZ); + }*/ + if(laserSW >= 2 && awayFlag == 3) + { + laserSW = 0; + awayCode[0] = x; + awayCode[1] = y+200; + awayCode[2] = angleZ; + awayFlag = 4; + path_size = makePath(awayCode[0], awayCode[1], awayCode[2], x, y, angleZ); + } + + //目標位置取得 + int targetX = PATH[path][0]; + int targetY = PATH[path][1]; + int targetR = PATH[path][2]; + + //経路更新 + path++; + if(path >= path_size)path = path_size-1; + + //目標速度ベクトル計算 + float xd = targetX - x; + float yd = targetY - y; + float r = sqrt(xd*xd + yd*yd); + float fai = atan2(yd, xd) - angleZ*DEG_TO_RAD; + float target_dx = r*cos(fai); + float target_dy = r*sin(fai); + vx = pid_x.controlPID(target_dx, 0); + vy = pid_y.controlPID(target_dy, 0); + vo = pid_yaw.controlPID(targetR, angleZ); + + if(awayFlag == 1) + { + if(abs(awayCode[0]-x)<5 && abs(awayCode[1]-y)<5 && abs(awayCode[2]-angleZ)<2 && abs(vo)<2) + { + //車輪の停止(念のため5回) + for(int _ = 0; _ < 5; _++) + { + writeSomeData(Arduino_Address, TARGET_X, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_Y, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_R, 0, 4); + wait_ms(1); + } + awayFlag = 2; + path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ); + path = 0; + + //waitがあるのでジャイロのリセット + jy901.reset(); + //一応pidもリセットしとく + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + } + else + { + writeSomeData(Arduino_Address, TARGET_X, vx, 4); + writeSomeData(Arduino_Address, TARGET_Y, vy, 4); + writeSomeData(Arduino_Address, TARGET_R, vo, 4); + } + } + else if(awayFlag == 4) + { + if(abs(awayCode[0]-x)<5 && abs(awayCode[1]-y)<5 && abs(awayCode[2]-angleZ)<2 && abs(vo)<2) + { + //車輪の停止(念のため5回) + motorStop(); + awayFlag = 0; + path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ); + path = 0; + //waitがあるのでジャイロのリセット + jy901.reset(); + //一応pidもリセットしとく + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + } + else + { + writeSomeData(Arduino_Address, TARGET_X, vx, 4); + writeSomeData(Arduino_Address, TARGET_Y, vy, 4); + writeSomeData(Arduino_Address, TARGET_R, vo, 4); + } + } + + else if(abs(Gcode[0]-x)<5 && abs(Gcode[1]-y)<5 && abs(Gcode[2]-angleZ)<2 && abs(vo)<2) + { + //車輪の停止(念のため5回) + for(int _ = 0; _ < 5; _++) + { + writeSomeData(Arduino_Address, TARGET_X, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_Y, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_R, 0, 4); + wait_ms(1); + } + + //シーケンスを進める + seq++; + //Gコード取得 + code.getGcode(seq, Gcode_size, Gcode); + //次の地点までのパスを作成 + path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ); + //パスのリセット + path = 0; + + //waitがあるのでジャイロのリセット + jy901.reset(); + //一応pidもリセットしとく + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + } + else //目標地点に到達していないとき + { + writeSomeData(Arduino_Address, TARGET_X, vx, 4); + writeSomeData(Arduino_Address, TARGET_Y, vy, 4); + writeSomeData(Arduino_Address, TARGET_R, vo, 4); + } + LED = seq; + } + LED = 7; + motorStop(); + wait_ms(500); +} +void followMe() +{ + writeSomeData(Arduino_Address, TARGET_Z, 120, 4); + writeSomeData(Arduino_Address, SERVO_ENABLE, 1, 4); + wait_ms(1000); + pid_x.setParameter(1.0, 0, 0);//(6.0, 0.57851); + pid_y.setParameter(0.7, 0, 0);//(2.0, 0.7155); + pid_yaw.setParameter(7, 0.6579); + + int path = 0; + int path_size = 1; + + int modeAngle = 0; + + // + int guideX = 0, guideY = 150; + int targetX = 0; + int targetY = 0; + float angleZ = 0; + float targetR = 0; + float x = 0, y = 0; + float vx = 0, vy = 0, vo = 0; + + int followMode = 0; + + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + while(1) + { + //自己位置推定 + angleZ = jy901.calculateAngleOnlyGyro(); + float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r; + float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r; + x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD); + y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD); + + //ガイドロボット相対位置取得 + guideX = Register[FRONT_OBJ_POSITION]; + guideY = Register[FRONT_OBJ_DISTANCE]; + + if(y > 900 && followMode == 0) + { + followMode = 1; + } + if(followMode == 1) + { + pid_yaw.setParameter(1.5, 0, 0); + targetR = atan2(double(-guideX), double(guideY))*RAD_TO_DEG; + vx = 0; + vy = pid_y.controlPID(guideY, 150); + //vy = 0; + vo = pid_yaw.controlPID(targetR, 0); + if(angleZ > 40) + { + followMode = 2; + modeAngle = 90; + } + else if(angleZ < -40) + { + followMode = 2; + modeAngle = -90; + } + } + if(followMode == 2) + { + path_size = makePath(x, 1300, modeAngle, x, y, angleZ); + followMode = 3; + pid_yaw.reset(); + } + if(followMode == 3) + { + pid_yaw.setParameter(7, 0.6579); + //経路更新 + path++; + if(path >= path_size)path = path_size-1; + targetX = PATH[path][0]; + targetY = PATH[path][1]; + targetR = PATH[path][2]; + vx = pid_x.controlPID(targetX, x); + vy = pid_y.controlPID(targetY, y); + vo = pid_yaw.controlPID(targetR, angleZ); + if(y > 1280) + { + vx = 0; + vy = 0; + vo = 0; + followMode = 4; + } + } + if(followMode == 4 && x < -1200) + { + //followMode = 5; + } + if(followMode == 5) + { + pid_yaw.setParameter(1.5, 0, 0); + targetR = atan2(double(-guideX), double(guideY))*RAD_TO_DEG; + vx = 0; + //vy = pid_y.controlPID(targetY, 150); + vy = 0; + vo = pid_yaw.controlPID(targetR, 0); + if(angleZ > 130) + { + path_size = makePath(-1500, y, 180, x, y, angleZ); + path = 0; + followMode = 6; + pid_yaw.reset(); + } + } + if(followMode == 6) + { + pid_yaw.setParameter(7, 0.6579); + //経路更新 + path++; + if(path >= path_size)path = path_size-1; + targetX = PATH[path][0]; + targetY = PATH[path][1]; + targetR = PATH[path][2]; + vx = pid_x.controlPID(targetX, x); + vy = pid_y.controlPID(targetY, y); + vo = pid_yaw.controlPID(targetR, angleZ); + if(x < -1450) + { + vx = 0; + vy = 0; + vo = 0; + followMode = 7; + } + } + if(guideY < 1) + { + vx = 0; + vy = 0; + } + else if(followMode == 0 || followMode == 4 || followMode == 7) + { + //目標速度 + vx = pid_x.controlPID(guideX, 0); + vy = pid_y.controlPID(guideY, 150); + vo = pid_yaw.controlPID(targetR, angleZ); + } + + writeSomeData(Arduino_Address, TARGET_X, vx, 4); + writeSomeData(Arduino_Address, TARGET_Y, vy, 4); + writeSomeData(Arduino_Address, TARGET_R, vo, 4); + //wait_ms(30); + } +} + +void pickAndPlace(FILE *fp) +{ + MakeSequencer code(fp); + const int seq_size = code.getGcodeSize(); + int Gcode[Gcode_size] = {}; + code.getGcode(0, Gcode_size, Gcode); + for(int i = 0; i < Gcode_size; i++) + printf("%d, ", Gcode[i]); + printf("\r\n"); + + pid_x.setParameter(1.5, 0.57851);//(6.0, 0.57851); + pid_y.setParameter(2.0, 0.58);//(2.0, 0.7155); + pid_yaw.setParameter(7, 0.6579); + + float angleZ = 0; + float x = 0, y = 0; + + int linePos = line.getPosition(); + int last_linePos = linePos; + int d_linePos = 0; + + int seq = 0; + int path = 0; + + int targetZ = 0; + int hand = 10; + int last_targetZ = 0; + int last_hand = 10; + + int controlFlag = 1; + + int path_size = makePath(Gcode[seq], Gcode[seq], Gcode[seq], x, y, angleZ); + + jy901.reset(); + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + + float last_t_lineRead = 0; + + while(seq < seq_size) + { + //自己位置推定 + angleZ = jy901.calculateAngleOnlyGyro(); + float dx = RE[0].getDisAngle() * DEG_TO_RAD * wheel_r; + float dy = RE[1].getDisAngle() * DEG_TO_RAD * wheel_r; + x += dx * cos(angleZ * DEG_TO_RAD) + dy * sin(-angleZ * DEG_TO_RAD); + y += dy * cos(angleZ * DEG_TO_RAD) + dx * sin(angleZ * DEG_TO_RAD); + + //printf("%f, %f\r\n", x, y); + + if(timer2.read() - last_t_lineRead > 0.100) + { + last_linePos = linePos; + linePos = line.getPosition(); + d_linePos = linePos - last_linePos; + last_t_lineRead = timer2.read(); + } + + //目標位置取得 + int targetX = PATH[path][0]; + int targetY = PATH[path][1]; + int targetR = PATH[path][2]; + //経路更新 + path++; + if(path >= path_size)path = path_size-1; + + //目標速度ベクトル計算 + float xd = targetX - x; + float yd = targetY - y; + float r = sqrt(xd*xd + yd*yd); + float fai = atan2(yd, xd) - angleZ*DEG_TO_RAD; + float target_dx = r*cos(fai); + float target_dy = r*sin(fai); + float vx = pid_x.controlPID(target_dx, 0); + float vy = pid_y.controlPID(target_dy, 0); + float vo = pid_yaw.controlPID(targetR, angleZ); + + //printf("%f, %f, %f\r\n", x, y, angleZ); + //printf("%f:%d, %f:%d, %f:%d\r\n", x, targetX, y, targetY, angleZ, targetR); + //printf("%d, %d, %d\r\n", targetX, targetY, targetR); + //printf("%f, %f, %f\r\n", vx, vy, vo); + + //目標地点に到達したときの処理 + //if(abs(Gcode[seq][0]-x)<5 && abs(Gcode[seq][1]-y)<5 && abs(Gcode[seq][2]-angleZ)<3) + if(abs(Gcode[0]-x)<5 && abs(Gcode[1]-y)<5 && abs(Gcode[2]-angleZ)<10) + { + //車輪の停止(念のため5回) + for(int _ = 0; _ < 1; _++) + { + writeSomeData(Arduino_Address, TARGET_X, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_Y, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_R, 0, 4); + wait_ms(1); + } + + last_targetZ = targetZ; + last_hand = hand; + targetZ = Gcode[3]; + hand = Gcode[4]; + + if(Gcode[5] == 1) + { + alarm(3); + } + if(targetZ != last_targetZ) + { + printf("%d\r\n", targetZ); + writeSomeData(Arduino_Address, TARGET_Z, targetZ, 4); + writeSomeData(Arduino_Address, SERVO_ENABLE, 1, 4); + controlFlag = 0; + wait_ms(abs(targetZ-last_targetZ)*20); + } + if(hand != last_hand) + { + writeSomeData(Arduino_Address, TARGET_H, Gcode[4], 4); + controlFlag = 0; + wait_ms(1000); + } + + //シーケンスを進める + seq++; + //Gコード取得 + code.getGcode(seq, Gcode_size, Gcode); + for(int i = 0; i < Gcode_size; i++) + printf("%d, ", Gcode[i]); + printf("\r\n"); + //次の地点までのパスを作成 + path_size = makePath(Gcode[0], Gcode[1], Gcode[2], x, y, angleZ); + //パスのリセット + path = 0; + + //waitがあるのでジャイロのリセット + jy901.reset(); + //一応pidもリセットしとく + pid_x.reset(); + pid_y.reset(); + pid_yaw.reset(); + } + else //目標地点に到達していないとき + { + writeSomeData(Arduino_Address, TARGET_X, vx, 4); + writeSomeData(Arduino_Address, TARGET_Y, vy, 4); + writeSomeData(Arduino_Address, TARGET_R, vo, 4); + } + } +} + +void xbeeControll() +{ + PacketSerial xbee(USBTX, USBRX, Register, 115200); + + while(1) + { + } +} + +enum TASK +{ + ROBOT_GUIDE = 6, //110 + FOLLOW_ME = 5, //101 + PICK_AND_PLACE = 3, //011 + CONTROLL = 7 //111 +}; + +int main() +{ + for(int _ = 0; _ < 5; _++) + { + writeSomeData(Arduino_Address, TARGET_X, 0, 4); + writeSomeData(Arduino_Address, TARGET_Y, 0, 4); + writeSomeData(Arduino_Address, TARGET_R, 0, 4); + } + LED = 0; + buzzer = 0; + + line.setBarStrobe(); + line.clearInvertBits(); + uint8_t returnStatus = line.begin(); + jy901.calibrateAll(5000); + + bool buttonFlag = 0; + int buttonTime = 0; + int mode = 1; + int task = 0; + alarm(); + while(1) + { + LED = mode; + task = modeChange.read(); + //押してないとき + if(button == 1) + { + if(timer.read()-buttonTime < 1.0 && buttonFlag == 0) + { + mode++; + if(mode >= 5)mode = 0; + wait_ms(300); + } + buttonFlag = 1; + + } + //押したとき + else + { + if(buttonFlag == 1) + { + buttonTime = timer.read(); + } + if(timer.read()-buttonTime > 1.5) + { + //実行処理 + LED = 0; + switch(task) + { + case CONTROLL: + FILE *demofp = fopen( "/local/demo.txt", "r"); + pickAndPlace(demofp); + fclose(demofp); + break; + case ROBOT_GUIDE: + FILE *robotfp; + switch(mode) + { + case 1: + robotfp = fopen( "/local/robot1.txt", "r"); + break; + case 2: + robotfp = fopen( "/local/robot2.txt", "r"); + break; + case 3: + robotfp = fopen( "/local/robot3.txt", "r"); + break; + case 4: + robotfp = fopen( "/local/robot4.txt", "r"); + break; + } + robotGuide(robotfp); + break; + case FOLLOW_ME: + followMe(); + break; + case PICK_AND_PLACE: + + FILE *fp; + switch(mode) + { + case 1: + fp = fopen( "/local/guide1.txt", "r"); + break; + case 2: + fp = fopen( "/local/guide2.txt", "r"); + break; + case 3: + fp = fopen( "/local/guide3.txt", "r"); + break; + case 4: + fp = fopen( "/local/guide4.txt", "r"); + break; + + default: + fp = fopen( "/local/guide1.txt", "r"); + break; + } + pickAndPlace(fp); + break; + + default: + alarm(2); + break; + } + } + buttonFlag = 0; + } + } +} + +bool writeSomeData(char addr, char reg, int32_t data ,int size) +{ + char tmp[2+size]; + tmp[0] = reg; + tmp[1] = size; + for(int i = 0; i < size; i++) + { + tmp[i+2] = (data >> (i*8)) & 0xFF; + } + //char DATA[2] = {reg,size}; + bool N = i2c.write(addr, tmp, 2+size); + //N|= i2c.write(addr, tmp, size); + return N; +} + +int makePath(int targetX, int targetY, int targetR, int x, int y, int angleZ) +{ + int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetR-angleZ) ) / 1.0; //1mm間隔 + //printf("num = %d\r\n", num); + for(int i = 1; i <= num; i++) + { + float a = float(i) / num; + PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); + PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); + PATH[i-1][2] = angleZ + float(targetR - angleZ) * a; + } + if(num > 0) + { + PATH[num-1][0] = targetX; + PATH[num-1][1] = targetY; + PATH[num-1][2] = targetR; + } + else + { + PATH[0][0] = targetX; + PATH[0][1] = targetY; + PATH[0][2] = targetR; + num = 1; + } + return num; +} + +void alarm(int n) +{ + int frequency[] = {NOTE_G5, NOTE_C5, NOTE_E5, NOTE_AS5, NOTE_DS5};//, 1, NOTE_GS5, NOTE_CS5, NOTE_F5, NOTE_B5, NOTE_E5, 1}; + int beat[] = {16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16}; + for (int i = 0; i < n; i++) + { + buzzer.period(1.0 / frequency[i]); + buzzer.write(0.5); + wait(1.0 / beat[i]); + buzzer.write(0); + wait(0.05); + } + buzzer = 0; +} +void motorStop() +{ + for(int _ = 0; _ < 5; _++) + { + writeSomeData(Arduino_Address, TARGET_X, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_Y, 0, 4); + wait_ms(1); + writeSomeData(Arduino_Address, TARGET_R, 0, 4); + wait_ms(1); + } +} + +float sign(float val) +{ + if(val > 0) + { + return 1.0; + } + else if(val < 0) + { + return -1.0; + } + else + { + return 0; + } +} \ No newline at end of file