hidaka sato / Mbed 2 deprecated WRS2019_master

WRS2019

Dependencies:   mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3

main.cpp@0:f1459eec7228, 2019-12-16 (annotated)

Committer:
sgrsn
Date:
Mon Dec 16 10:38:07 2019 +0000
Revision:
0:f1459eec7228
Child:
1:f102831401a8
First

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:f1459eec7228 1 #include "mbed.h"
sgrsn 0:f1459eec7228 2 #include <string>
sgrsn 0:f1459eec7228 3 #include "i2cmaster.h"
sgrsn 0:f1459eec7228 4 #include "JY901.h"
sgrsn 0:f1459eec7228 5 #include "PID.h"
sgrsn 0:f1459eec7228 6 #include "MakeSequencer.h"
sgrsn 0:f1459eec7228 7 #include "define.h"
sgrsn 0:f1459eec7228 8
sgrsn 0:f1459eec7228 9 #include "TextLCD.h"
sgrsn 0:f1459eec7228 10
sgrsn 0:f1459eec7228 11
sgrsn 0:f1459eec7228 12 // MakeSequencer
sgrsn 0:f1459eec7228 13 #define SIZE 6
sgrsn 0:f1459eec7228 14 #define ArraySize(array) (sizeof(array) / sizeof(array[0]))
sgrsn 0:f1459eec7228 15
sgrsn 0:f1459eec7228 16 float DEG_TO_RAD = PI/180.0;
sgrsn 0:f1459eec7228 17
sgrsn 0:f1459eec7228 18 void controlMotor(int ch, int frequency);
sgrsn 0:f1459eec7228 19 void coastAllMotor();
sgrsn 0:f1459eec7228 20 void controlFrequencyFromTerminal();
sgrsn 0:f1459eec7228 21 void serialRead();
sgrsn 0:f1459eec7228 22
sgrsn 0:f1459eec7228 23 int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
sgrsn 0:f1459eec7228 24 int Register[0x20] = {};
sgrsn 0:f1459eec7228 25
sgrsn 0:f1459eec7228 26 Serial PC(USBTX, USBRX);
sgrsn 0:f1459eec7228 27 i2c master(p28, p27);
sgrsn 0:f1459eec7228 28 BusOut LEDs(LED1, LED2, LED3, LED4);
sgrsn 0:f1459eec7228 29 Timer timer;
sgrsn 0:f1459eec7228 30 JY901 jy901(&master, &timer);
sgrsn 0:f1459eec7228 31
sgrsn 0:f1459eec7228 32
sgrsn 0:f1459eec7228 33 void controlFromGcode()
sgrsn 0:f1459eec7228 34 {
sgrsn 0:f1459eec7228 35 float threshold_x = 0; //[mm]
sgrsn 0:f1459eec7228 36 float threshold_y = 0; //[mm]
sgrsn 0:f1459eec7228 37 float threshold_theta = 5 * DEG_TO_RAD;
sgrsn 0:f1459eec7228 38
sgrsn 0:f1459eec7228 39 // 角度補正係数
sgrsn 0:f1459eec7228 40 float L = 233; //[mm]
sgrsn 0:f1459eec7228 41
sgrsn 0:f1459eec7228 42 Timer timer2;
sgrsn 0:f1459eec7228 43 PID pid_x(&timer2);
sgrsn 0:f1459eec7228 44 PID pid_y(&timer2);
sgrsn 0:f1459eec7228 45 PID pid_theta(&timer2);
sgrsn 0:f1459eec7228 46 pid_x.setParameter(100.0, 0.0, 0.0);
sgrsn 0:f1459eec7228 47 pid_y.setParameter(100.0, 0.0, 0.0);
sgrsn 0:f1459eec7228 48 pid_theta.setParameter(1.0, 0.0, 0.0);
sgrsn 0:f1459eec7228 49
sgrsn 0:f1459eec7228 50 // Gコード読み取り
sgrsn 0:f1459eec7228 51 LocalFileSystem local("local");
sgrsn 0:f1459eec7228 52 int array[SIZE] = {};
sgrsn 0:f1459eec7228 53 FILE *fp = fopen( "/local/guide1.txt", "r");
sgrsn 0:f1459eec7228 54 MakeSequencer code(fp);
sgrsn 0:f1459eec7228 55
sgrsn 0:f1459eec7228 56 int row = 1;
sgrsn 0:f1459eec7228 57 float v[4] = {};
sgrsn 0:f1459eec7228 58
sgrsn 0:f1459eec7228 59 TextLCD lcd(p24, p26, p27, p28, p29, p30);
sgrsn 0:f1459eec7228 60
sgrsn 0:f1459eec7228 61 while(1)
sgrsn 0:f1459eec7228 62 {
sgrsn 0:f1459eec7228 63
sgrsn 0:f1459eec7228 64 // 自己位置推定
sgrsn 0:f1459eec7228 65 float x_robot = Register[MARKER_X];
sgrsn 0:f1459eec7228 66 float y_robot = Register[MARKER_Y];
sgrsn 0:f1459eec7228 67 float theta_robot = float(Register[MARKER_YAW]) / 1000.0;
sgrsn 0:f1459eec7228 68 float theta_robot_formJyro = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 0:f1459eec7228 69
sgrsn 0:f1459eec7228 70 lcd.printf("%d,%d,%d\n", (int)x_robot, (int)y_robot, (int)(theta_robot*180/PI));
sgrsn 0:f1459eec7228 71
sgrsn 0:f1459eec7228 72 // 目標位置把握
sgrsn 0:f1459eec7228 73 code.getGcode(row,sizeof(array)/sizeof(int),array);
sgrsn 0:f1459eec7228 74 float x_desire = array[0];
sgrsn 0:f1459eec7228 75 float y_desire = array[1];
sgrsn 0:f1459eec7228 76 float theta_desire = float(array[2]) *DEG_TO_RAD;
sgrsn 0:f1459eec7228 77
sgrsn 0:f1459eec7228 78 // 目標位置判定
sgrsn 0:f1459eec7228 79 float err_x = x_desire - x_robot;
sgrsn 0:f1459eec7228 80 float err_y = y_desire - y_robot;
sgrsn 0:f1459eec7228 81 float err_theta = theta_desire - theta_robot;
sgrsn 0:f1459eec7228 82
sgrsn 0:f1459eec7228 83 // 目標位置到達
sgrsn 0:f1459eec7228 84 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
sgrsn 0:f1459eec7228 85 {
sgrsn 0:f1459eec7228 86 // 車輪を停止
sgrsn 0:f1459eec7228 87 coastAllMotor();
sgrsn 0:f1459eec7228 88
sgrsn 0:f1459eec7228 89 // Gコードを次の行に
sgrsn 0:f1459eec7228 90 row++;
sgrsn 0:f1459eec7228 91 code.getGcode(row, ArraySize(array), array);
sgrsn 0:f1459eec7228 92 }
sgrsn 0:f1459eec7228 93
sgrsn 0:f1459eec7228 94 // 目標速度計算
sgrsn 0:f1459eec7228 95 else
sgrsn 0:f1459eec7228 96 {
sgrsn 0:f1459eec7228 97 // 慣性座標での速度
sgrsn 0:f1459eec7228 98 float xI_dot = pid_x.controlPID(x_desire, x_robot);
sgrsn 0:f1459eec7228 99 float yI_dot = pid_y.controlPID(y_desire, y_robot);
sgrsn 0:f1459eec7228 100 float theta_dot = pid_theta.controlPID(theta_desire, theta_robot);
sgrsn 0:f1459eec7228 101
sgrsn 0:f1459eec7228 102 // ロボット座標での速度
sgrsn 0:f1459eec7228 103 float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 104 float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 105
sgrsn 0:f1459eec7228 106 // 各車輪の速度
sgrsn 0:f1459eec7228 107 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 108 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 109 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 110 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 111
sgrsn 0:f1459eec7228 112 // 本当はこうするべき
sgrsn 0:f1459eec7228 113 // f = v * ppr / ( 2*PI * r);
sgrsn 0:f1459eec7228 114
sgrsn 0:f1459eec7228 115 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 116 {
sgrsn 0:f1459eec7228 117 controlMotor(i, (int)v[i] );
sgrsn 0:f1459eec7228 118 }
sgrsn 0:f1459eec7228 119 }
sgrsn 0:f1459eec7228 120 }
sgrsn 0:f1459eec7228 121 }
sgrsn 0:f1459eec7228 122
sgrsn 0:f1459eec7228 123 int main()
sgrsn 0:f1459eec7228 124 {
sgrsn 0:f1459eec7228 125 coastAllMotor();
sgrsn 0:f1459eec7228 126 PC.baud(9600);
sgrsn 0:f1459eec7228 127 PC.attach(serialRead);
sgrsn 0:f1459eec7228 128 //jy901.calibrateAll(5000);
sgrsn 0:f1459eec7228 129
sgrsn 0:f1459eec7228 130 controlFromGcode();
sgrsn 0:f1459eec7228 131 }
sgrsn 0:f1459eec7228 132
sgrsn 0:f1459eec7228 133
sgrsn 0:f1459eec7228 134 void controlMotor(int ch, int frequency)
sgrsn 0:f1459eec7228 135 {
sgrsn 0:f1459eec7228 136 int dir = COAST;
sgrsn 0:f1459eec7228 137 int size = 4;
sgrsn 0:f1459eec7228 138 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 139 {
sgrsn 0:f1459eec7228 140 //channel error
sgrsn 0:f1459eec7228 141 }
sgrsn 0:f1459eec7228 142 else
sgrsn 0:f1459eec7228 143 {
sgrsn 0:f1459eec7228 144 if(frequency > 0)
sgrsn 0:f1459eec7228 145 {
sgrsn 0:f1459eec7228 146 dir = CW;
sgrsn 0:f1459eec7228 147 }
sgrsn 0:f1459eec7228 148 else if(frequency < 0)
sgrsn 0:f1459eec7228 149 {
sgrsn 0:f1459eec7228 150 dir = CCW;
sgrsn 0:f1459eec7228 151 frequency = -frequency;
sgrsn 0:f1459eec7228 152 }
sgrsn 0:f1459eec7228 153 else
sgrsn 0:f1459eec7228 154 {
sgrsn 0:f1459eec7228 155 dir = BRAKE;
sgrsn 0:f1459eec7228 156 }
sgrsn 0:f1459eec7228 157 // 周波数制限 脱調を防ぐ
sgrsn 0:f1459eec7228 158 if(frequency > MaxFrequency) frequency = MaxFrequency;
sgrsn 0:f1459eec7228 159
sgrsn 0:f1459eec7228 160 master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
sgrsn 0:f1459eec7228 161 master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
sgrsn 0:f1459eec7228 162 }
sgrsn 0:f1459eec7228 163 }
sgrsn 0:f1459eec7228 164
sgrsn 0:f1459eec7228 165
sgrsn 0:f1459eec7228 166 void coastAllMotor()
sgrsn 0:f1459eec7228 167 {
sgrsn 0:f1459eec7228 168 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 169 {
sgrsn 0:f1459eec7228 170 master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
sgrsn 0:f1459eec7228 171 }
sgrsn 0:f1459eec7228 172 }
sgrsn 0:f1459eec7228 173
sgrsn 0:f1459eec7228 174 void serialRead()
sgrsn 0:f1459eec7228 175 {
sgrsn 0:f1459eec7228 176 int reg = 0;
sgrsn 0:f1459eec7228 177 uint8_t data[4] = {};
sgrsn 0:f1459eec7228 178 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 179 {
sgrsn 0:f1459eec7228 180 reg = PC.getc();
sgrsn 0:f1459eec7228 181 data[0] = PC.getc();
sgrsn 0:f1459eec7228 182 data[1] = PC.getc();
sgrsn 0:f1459eec7228 183 data[2] = PC.getc();
sgrsn 0:f1459eec7228 184 data[3] = PC.getc();
sgrsn 0:f1459eec7228 185 }
sgrsn 0:f1459eec7228 186 Register[reg % 0x20] = 0;
sgrsn 0:f1459eec7228 187 for(int i = 0; i < 4; i++)
sgrsn 0:f1459eec7228 188 Register[reg % 0x20] |= int(data[i]) << (i*8);
sgrsn 0:f1459eec7228 189 }
sgrsn 0:f1459eec7228 190
sgrsn 0:f1459eec7228 191
sgrsn 0:f1459eec7228 192
sgrsn 0:f1459eec7228 193 /*Function for Test***********************************************************/
sgrsn 0:f1459eec7228 194
sgrsn 0:f1459eec7228 195 void controlFrequencyFromTerminal()
sgrsn 0:f1459eec7228 196 {
sgrsn 0:f1459eec7228 197 int ch, speed;
sgrsn 0:f1459eec7228 198 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 199 {
sgrsn 0:f1459eec7228 200 PC.scanf("M%d:%d", &ch, &speed);
sgrsn 0:f1459eec7228 201 PC.printf("M%d:%d\r\n", ch, speed);
sgrsn 0:f1459eec7228 202 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 203 PC.printf("channnel error\r\n");
sgrsn 0:f1459eec7228 204 else
sgrsn 0:f1459eec7228 205 {
sgrsn 0:f1459eec7228 206 controlMotor(ch, speed);
sgrsn 0:f1459eec7228 207 }
sgrsn 0:f1459eec7228 208 }
sgrsn 0:f1459eec7228 209 }
sgrsn 0:f1459eec7228 210
sgrsn 0:f1459eec7228 211 void controlFromWASD()
sgrsn 0:f1459eec7228 212 {
sgrsn 0:f1459eec7228 213 float L = 4.0;
sgrsn 0:f1459eec7228 214 float v[4] = {};
sgrsn 0:f1459eec7228 215 char input = 0;
sgrsn 0:f1459eec7228 216 while(1)
sgrsn 0:f1459eec7228 217 {
sgrsn 0:f1459eec7228 218 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 219 {
sgrsn 0:f1459eec7228 220 input = PC.getc();
sgrsn 0:f1459eec7228 221 //printf("%c\r\n", input);
sgrsn 0:f1459eec7228 222 }
sgrsn 0:f1459eec7228 223
sgrsn 0:f1459eec7228 224 float xI_dot = 0;
sgrsn 0:f1459eec7228 225 float yI_dot = 0;
sgrsn 0:f1459eec7228 226 switch(input)
sgrsn 0:f1459eec7228 227 {
sgrsn 0:f1459eec7228 228 case 'a':
sgrsn 0:f1459eec7228 229 xI_dot = -1;
sgrsn 0:f1459eec7228 230 yI_dot = 0;
sgrsn 0:f1459eec7228 231 break;
sgrsn 0:f1459eec7228 232 case 'd':
sgrsn 0:f1459eec7228 233 xI_dot = 1;
sgrsn 0:f1459eec7228 234 yI_dot = 0;
sgrsn 0:f1459eec7228 235 break;
sgrsn 0:f1459eec7228 236 case 'w':
sgrsn 0:f1459eec7228 237 xI_dot = 0;
sgrsn 0:f1459eec7228 238 yI_dot = 1;
sgrsn 0:f1459eec7228 239 break;
sgrsn 0:f1459eec7228 240 case 's':
sgrsn 0:f1459eec7228 241 xI_dot = 0;
sgrsn 0:f1459eec7228 242 yI_dot = -1;
sgrsn 0:f1459eec7228 243 break;
sgrsn 0:f1459eec7228 244 case ' ':
sgrsn 0:f1459eec7228 245 xI_dot = 0;
sgrsn 0:f1459eec7228 246 yI_dot = 0;
sgrsn 0:f1459eec7228 247 break;
sgrsn 0:f1459eec7228 248 }
sgrsn 0:f1459eec7228 249 //master.getSlaveRegistarData(addr, 1, &data, size);
sgrsn 0:f1459eec7228 250
sgrsn 0:f1459eec7228 251 float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 0:f1459eec7228 252
sgrsn 0:f1459eec7228 253 float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 254 float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 255 float theta_dot = 0.0 - theta_z;
sgrsn 0:f1459eec7228 256
sgrsn 0:f1459eec7228 257 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 258 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 259 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 260 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 261
sgrsn 0:f1459eec7228 262 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 263 {
sgrsn 0:f1459eec7228 264 controlMotor(i, v[i] * 20000.0);
sgrsn 0:f1459eec7228 265 }
sgrsn 0:f1459eec7228 266
sgrsn 0:f1459eec7228 267 PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]);
sgrsn 0:f1459eec7228 268
sgrsn 0:f1459eec7228 269 //PC.printf("%f\r\n", theta_z);
sgrsn 0:f1459eec7228 270 }
sgrsn 0:f1459eec7228 271 }