Revision 0:08b48d8e6eab, committed 2018-11-14

Comitter:

sgrsn

Date:

Wed Nov 14 02:23:08 2018 +0000

Commit message:

First Commit

Changed in this revision

diff -r 000000000000 -r 08b48d8e6eab JY901.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JY901.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/sgrsn/code/JY901/#7e7dd6184774

diff -r 000000000000 -r 08b48d8e6eab Make_Sequencer_3.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Make_Sequencer_3.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/Make_Sequencer_3/#e513810fc3a6

diff -r 000000000000 -r 08b48d8e6eab PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/sgrsn/code/PID/#6949e401a9ad

diff -r 000000000000 -r 08b48d8e6eab PacketSerial.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PacketSerial.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/sgrsn/code/PacketSerial/#ea92ff9abc8c

diff -r 000000000000 -r 08b48d8e6eab QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/QEI_simple/#6fa863d09d45

diff -r 000000000000 -r 08b48d8e6eab SensorBar.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorBar.lib	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/SensorBar/#3a86ce26147b

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

diff -r 000000000000 -r 08b48d8e6eab mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file

diff -r 000000000000 -r 08b48d8e6eab pitches.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pitches.h	Wed Nov 14 02:23:08 2018 +0000
@@ -0,0 +1,93 @@
+/*************************************************
+ * Public Constants
+ *************************************************/
+
+#define NOTE_B0  31
+#define NOTE_C1  33
+#define NOTE_CS1 35
+#define NOTE_D1  37
+#define NOTE_DS1 39
+#define NOTE_E1  41
+#define NOTE_F1  44
+#define NOTE_FS1 46
+#define NOTE_G1  49
+#define NOTE_GS1 52
+#define NOTE_A1  55
+#define NOTE_AS1 58
+#define NOTE_B1  62
+#define NOTE_C2  65
+#define NOTE_CS2 69
+#define NOTE_D2  73
+#define NOTE_DS2 78
+#define NOTE_E2  82
+#define NOTE_F2  87
+#define NOTE_FS2 93
+#define NOTE_G2  98
+#define NOTE_GS2 104
+#define NOTE_A2  110
+#define NOTE_AS2 117
+#define NOTE_B2  123
+#define NOTE_C3  131
+#define NOTE_CS3 139
+#define NOTE_D3  147
+#define NOTE_DS3 156
+#define NOTE_E3  165
+#define NOTE_F3  175
+#define NOTE_FS3 185
+#define NOTE_G3  196
+#define NOTE_GS3 208
+#define NOTE_A3  220
+#define NOTE_AS3 233
+#define NOTE_B3  247
+#define NOTE_C4  262
+#define NOTE_CS4 277
+#define NOTE_D4  294
+#define NOTE_DS4 311
+#define NOTE_E4  330
+#define NOTE_F4  349
+#define NOTE_FS4 370
+#define NOTE_G4  392
+#define NOTE_GS4 415
+#define NOTE_A4  440
+#define NOTE_AS4 466
+#define NOTE_B4  494
+#define NOTE_C5  523
+#define NOTE_CS5 554
+#define NOTE_D5  587
+#define NOTE_DS5 622
+#define NOTE_E5  659
+#define NOTE_F5  698
+#define NOTE_FS5 740
+#define NOTE_G5  784
+#define NOTE_GS5 831
+#define NOTE_A5  880
+#define NOTE_AS5 932
+#define NOTE_B5  988
+#define NOTE_C6  1047
+#define NOTE_CS6 1109
+#define NOTE_D6  1175
+#define NOTE_DS6 1245
+#define NOTE_E6  1319
+#define NOTE_F6  1397
+#define NOTE_FS6 1480
+#define NOTE_G6  1568
+#define NOTE_GS6 1661
+#define NOTE_A6  1760
+#define NOTE_AS6 1865
+#define NOTE_B6  1976
+#define NOTE_C7  2093
+#define NOTE_CS7 2217
+#define NOTE_D7  2349
+#define NOTE_DS7 2489
+#define NOTE_E7  2637
+#define NOTE_F7  2794
+#define NOTE_FS7 2960
+#define NOTE_G7  3136
+#define NOTE_GS7 3322
+#define NOTE_A7  3520
+#define NOTE_AS7 3729
+#define NOTE_B7  3951
+#define NOTE_C8  4186
+#define NOTE_CS8 4435
+#define NOTE_D8  4699
+#define NOTE_DS8 4978