hidaka sato
For our Robot
Dependencies: JY901 Make_Sequencer_3 PID PacketSerial QEI_simple SensorBar mbed
Diff: main.cpp
- Revision:
- 0:08b48d8e6eab
--- /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