hidaka sato
WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
Revision 0:f1459eec7228, committed 2019-12-16
- Comitter:
- sgrsn
- Date:
- Mon Dec 16 10:38:07 2019 +0000
- Child:
- 1:f102831401a8
- Commit message:
- First
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JY901.lib Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sgrsn/code/JY901/#7e7dd6184774
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Make_Sequencer_3.lib Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sgrsn/code/Make_Sequencer_3/#41921eecf668
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sgrsn/code/PID2/#873985df821c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/TextLCD/#e4cb7ddee0d3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/define.h Mon Dec 16 10:38:07 2019 +0000
@@ -0,0 +1,43 @@
+/*Common for Master and Slave***************/
+
+typedef enum
+{
+ RightForward = 1,
+ LeftForward = 2,
+ RightBack = 3,
+ LeftBack = 4
+}MotorPosition;
+
+typedef enum
+{
+ COAST = 0,
+ BRAKE = 1,
+ CW = 2,
+ CCW = 3
+}MotorState;
+
+#define WHO_AM_I 0x00
+#define MY_IIC_ADDR 0x01
+#define MOTOR_DIR 0x04
+#define PWM_FREQUENCY 0x05
+
+
+/*Master only**************************************/
+
+#define MOTOR_NUM 4
+#define IIC_ADDR1 0xB0
+#define IIC_ADDR2 0xC0
+#define IIC_ADDR3 0xD0
+#define IIC_ADDR4 0xE0
+
+#define MaxFrequency 70000
+
+// Register Map from PC
+#define MARKER_X 0x05
+#define MARKER_Y 0x06
+#define MARKER_Z 0x07
+#define MARKER_ROLL 0x08
+#define MARKER_PITCH 0x09
+#define MARKER_YAW 0x010
+
+#define PI 3.141592654
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/i2cmaster.lib Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sgrsn/code/i2cmaster/#bc6d5a6e9fe1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Dec 16 10:38:07 2019 +0000
@@ -0,0 +1,271 @@
+#include "mbed.h"
+#include <string>
+#include "i2cmaster.h"
+#include "JY901.h"
+#include "PID.h"
+#include "MakeSequencer.h"
+#include "define.h"
+
+#include "TextLCD.h"
+
+
+// MakeSequencer
+#define SIZE 6
+#define ArraySize(array) (sizeof(array) / sizeof(array[0]))
+
+float DEG_TO_RAD = PI/180.0;
+
+void controlMotor(int ch, int frequency);
+void coastAllMotor();
+void controlFrequencyFromTerminal();
+void serialRead();
+
+int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
+int Register[0x20] = {};
+
+Serial PC(USBTX, USBRX);
+i2c master(p28, p27);
+BusOut LEDs(LED1, LED2, LED3, LED4);
+Timer timer;
+JY901 jy901(&master, &timer);
+
+
+void controlFromGcode()
+{
+ float threshold_x = 0; //[mm]
+ float threshold_y = 0; //[mm]
+ float threshold_theta = 5 * DEG_TO_RAD;
+
+ // 角度補正係数
+ float L = 233; //[mm]
+
+ Timer timer2;
+ PID pid_x(&timer2);
+ PID pid_y(&timer2);
+ PID pid_theta(&timer2);
+ pid_x.setParameter(100.0, 0.0, 0.0);
+ pid_y.setParameter(100.0, 0.0, 0.0);
+ pid_theta.setParameter(1.0, 0.0, 0.0);
+
+ // Gコード読み取り
+ LocalFileSystem local("local");
+ int array[SIZE] = {};
+ FILE *fp = fopen( "/local/guide1.txt", "r");
+ MakeSequencer code(fp);
+
+ int row = 1;
+ float v[4] = {};
+
+ TextLCD lcd(p24, p26, p27, p28, p29, p30);
+
+ while(1)
+ {
+
+ // 自己位置推定
+ float x_robot = Register[MARKER_X];
+ float y_robot = Register[MARKER_Y];
+ float theta_robot = float(Register[MARKER_YAW]) / 1000.0;
+ float theta_robot_formJyro = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
+
+ lcd.printf("%d,%d,%d\n", (int)x_robot, (int)y_robot, (int)(theta_robot*180/PI));
+
+ // 目標位置把握
+ code.getGcode(row,sizeof(array)/sizeof(int),array);
+ float x_desire = array[0];
+ float y_desire = array[1];
+ float theta_desire = float(array[2]) *DEG_TO_RAD;
+
+ // 目標位置判定
+ float err_x = x_desire - x_robot;
+ float err_y = y_desire - y_robot;
+ float err_theta = theta_desire - theta_robot;
+
+ // 目標位置到達
+ if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
+ {
+ // 車輪を停止
+ coastAllMotor();
+
+ // Gコードを次の行に
+ row++;
+ code.getGcode(row, ArraySize(array), array);
+ }
+
+ // 目標速度計算
+ else
+ {
+ // 慣性座標での速度
+ float xI_dot = pid_x.controlPID(x_desire, x_robot);
+ float yI_dot = pid_y.controlPID(y_desire, y_robot);
+ float theta_dot = pid_theta.controlPID(theta_desire, theta_robot);
+
+ // ロボット座標での速度
+ float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
+ float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
+
+ // 各車輪の速度
+ v[0] = -x_dot - y_dot - L * theta_dot;
+ v[1] = x_dot - y_dot - L * theta_dot;
+ v[2] = x_dot + y_dot - L * theta_dot;
+ v[3] = -x_dot + y_dot - L * theta_dot;
+
+ // 本当はこうするべき
+ // f = v * ppr / ( 2*PI * r);
+
+ for(int i = 0; i < MOTOR_NUM; i++)
+ {
+ controlMotor(i, (int)v[i] );
+ }
+ }
+ }
+}
+
+int main()
+{
+ coastAllMotor();
+ PC.baud(9600);
+ PC.attach(serialRead);
+ //jy901.calibrateAll(5000);
+
+ controlFromGcode();
+}
+
+
+void controlMotor(int ch, int frequency)
+{
+ int dir = COAST;
+ int size = 4;
+ if(ch < 0 || ch > 3)
+ {
+ //channel error
+ }
+ else
+ {
+ if(frequency > 0)
+ {
+ dir = CW;
+ }
+ else if(frequency < 0)
+ {
+ dir = CCW;
+ frequency = -frequency;
+ }
+ else
+ {
+ dir = BRAKE;
+ }
+ // 周波数制限 脱調を防ぐ
+ if(frequency > MaxFrequency) frequency = MaxFrequency;
+
+ master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
+ master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
+ }
+}
+
+
+void coastAllMotor()
+{
+ for(int i = 0; i < MOTOR_NUM; i++)
+ {
+ master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
+ }
+}
+
+void serialRead()
+{
+ int reg = 0;
+ uint8_t data[4] = {};
+ if(PC.readable() > 0)
+ {
+ reg = PC.getc();
+ data[0] = PC.getc();
+ data[1] = PC.getc();
+ data[2] = PC.getc();
+ data[3] = PC.getc();
+ }
+ Register[reg % 0x20] = 0;
+ for(int i = 0; i < 4; i++)
+ Register[reg % 0x20] |= int(data[i]) << (i*8);
+}
+
+
+
+/*Function for Test***********************************************************/
+
+void controlFrequencyFromTerminal()
+{
+ int ch, speed;
+ if(PC.readable() > 0)
+ {
+ PC.scanf("M%d:%d", &ch, &speed);
+ PC.printf("M%d:%d\r\n", ch, speed);
+ if(ch < 0 || ch > 3)
+ PC.printf("channnel error\r\n");
+ else
+ {
+ controlMotor(ch, speed);
+ }
+ }
+}
+
+void controlFromWASD()
+{
+ float L = 4.0;
+ float v[4] = {};
+ char input = 0;
+ while(1)
+ {
+ if(PC.readable() > 0)
+ {
+ input = PC.getc();
+ //printf("%c\r\n", input);
+ }
+
+ float xI_dot = 0;
+ float yI_dot = 0;
+ switch(input)
+ {
+ case 'a':
+ xI_dot = -1;
+ yI_dot = 0;
+ break;
+ case 'd':
+ xI_dot = 1;
+ yI_dot = 0;
+ break;
+ case 'w':
+ xI_dot = 0;
+ yI_dot = 1;
+ break;
+ case 's':
+ xI_dot = 0;
+ yI_dot = -1;
+ break;
+ case ' ':
+ xI_dot = 0;
+ yI_dot = 0;
+ break;
+ }
+ //master.getSlaveRegistarData(addr, 1, &data, size);
+
+ float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
+
+ float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot;
+ float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot;
+ float theta_dot = 0.0 - theta_z;
+
+ v[1] = x_dot - y_dot - L * theta_dot;
+ v[2] = x_dot + y_dot - L * theta_dot;
+ v[3] = -x_dot + y_dot - L * theta_dot;
+ v[0] = -x_dot - y_dot - L * theta_dot;
+
+ for(int i = 0; i < MOTOR_NUM; i++)
+ {
+ controlMotor(i, v[i] * 20000.0);
+ }
+
+ PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]);
+
+ //PC.printf("%f\r\n", theta_z);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 16 10:38:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file