hidaka sato
For our Robot
Dependencies: JY901 Make_Sequencer_3 PID PacketSerial QEI_simple SensorBar mbed
main.cpp
- Committer:
- sgrsn
- Date:
- 2018-11-14
- Revision:
- 0:08b48d8e6eab
File content as of revision 0:08b48d8e6eab:
#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;
}
}