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?
User | Revision | Line number | New 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 | } |