Akito / Mbed 2 deprecated ROBOCON2017_archan

あーちゃん制御

Dependencies:   FastPWM cal_PID mbed nucleo_rotary_encoder

main.cpp@1:d6e30aa7bc5b, 2017-09-25 (annotated)

Committer:
Akito914
Date:
Mon Sep 25 08:23:24 2017 +0000
Revision:
1:d6e30aa7bc5b
Parent:
0:4346c5764e83 
cal_PID???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Akito914 0:4346c5764e83 1 #include "mbed.h"
Akito914 0:4346c5764e83 2 #include "omuni.hpp"
Akito914 0:4346c5764e83 3 #include "cal_PID.hpp"
Akito914 0:4346c5764e83 4 #include "FastPWM.h"
Akito914 0:4346c5764e83 5
Akito914 0:4346c5764e83 6 const int comTimeout_ms = 200;
Akito914 0:4346c5764e83 7 const float omuni_speed_max = 2.3f;
Akito914 0:4346c5764e83 8 const float omuni_burst_coeff = 1.7f;
Akito914 0:4346c5764e83 9 const float omega_max = 1.0 * 2 * 3.14159265f;
Akito914 0:4346c5764e83 10 const float omega_f = 0.5 * 2 * 3.14159265f;
Akito914 0:4346c5764e83 11 const float wrap_radius = 1.0f;
Akito914 0:4346c5764e83 12 const float wrap_speed = 2.2f;
Akito914 0:4346c5764e83 13
Akito914 0:4346c5764e83 14 const int8_t armDuty[] = {-100, -100};
Akito914 0:4346c5764e83 15
Akito914 0:4346c5764e83 16 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
Akito914 0:4346c5764e83 17 const int armAddr[] = {0x16, 0x18}; // 1100 , 0010
Akito914 0:4346c5764e83 18 const int EMO_Addr = 0x1e; // 1110
Akito914 0:4346c5764e83 19
Akito914 0:4346c5764e83 20 int comTimeout_count = 0;
Akito914 0:4346c5764e83 21
Akito914 0:4346c5764e83 22 bool comTimeout_state = true;
Akito914 0:4346c5764e83 23
Akito914 0:4346c5764e83 24 bool ems = false;
Akito914 0:4346c5764e83 25 bool recovery = false;
Akito914 0:4346c5764e83 26
Akito914 0:4346c5764e83 27 bool arm = false;
Akito914 0:4346c5764e83 28
Akito914 0:4346c5764e83 29 DigitalIn button(USER_BUTTON);
Akito914 0:4346c5764e83 30 DigitalOut led(LED1);
Akito914 0:4346c5764e83 31
Akito914 0:4346c5764e83 32 Serial pc(USBTX, USBRX);
Akito914 0:4346c5764e83 33 Serial com(PA_11, PA_12);
Akito914 0:4346c5764e83 34 I2C i2cMaster(D14, D15);
Akito914 0:4346c5764e83 35 Omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
Akito914 0:4346c5764e83 36 Ticker comTimeout;
Akito914 0:4346c5764e83 37
Akito914 0:4346c5764e83 38 FastPWM soundOut(PB_7);
Akito914 0:4346c5764e83 39
Akito914 0:4346c5764e83 40 float speed_x, speed_y, omega;
Akito914 0:4346c5764e83 41 bool f;
Akito914 0:4346c5764e83 42
Akito914 0:4346c5764e83 43 typedef union{
Akito914 0:4346c5764e83 44 struct{
Akito914 0:4346c5764e83 45 unsigned startByte :8;
Akito914 0:4346c5764e83 46 unsigned joyLX :8;
Akito914 0:4346c5764e83 47 unsigned joyLY :8;
Akito914 0:4346c5764e83 48 unsigned joyRX :8;
Akito914 0:4346c5764e83 49 unsigned joyRY :8;
Akito914 0:4346c5764e83 50 unsigned L2 :8;
Akito914 0:4346c5764e83 51 unsigned R2 :8;
Akito914 0:4346c5764e83 52 unsigned arrowL :1;
Akito914 0:4346c5764e83 53 unsigned arrowR :1;
Akito914 0:4346c5764e83 54 unsigned arrowU :1;
Akito914 0:4346c5764e83 55 unsigned arrowD :1;
Akito914 0:4346c5764e83 56 unsigned L1 :1;
Akito914 0:4346c5764e83 57 unsigned R1 :1;
Akito914 0:4346c5764e83 58 unsigned SELECT :2;
Akito914 0:4346c5764e83 59 unsigned lift_gray :2;
Akito914 0:4346c5764e83 60 unsigned spear :1;
Akito914 0:4346c5764e83 61 unsigned arm :1;
Akito914 0:4346c5764e83 62 unsigned pushL :1;
Akito914 0:4346c5764e83 63 unsigned pushR :1;
Akito914 0:4346c5764e83 64 unsigned EMO :2;
Akito914 0:4346c5764e83 65 unsigned checksum :8;
Akito914 0:4346c5764e83 66 };
Akito914 0:4346c5764e83 67
Akito914 0:4346c5764e83 68 uint8_t list[10];
Akito914 0:4346c5764e83 69 }comBuf_t;
Akito914 0:4346c5764e83 70
Akito914 0:4346c5764e83 71 void received_processing(comBuf_t *buf);
Akito914 0:4346c5764e83 72 int byteSum(int8_t byte);
Akito914 0:4346c5764e83 73 bool checksum_check(comBuf_t *buf);
Akito914 0:4346c5764e83 74 int drive_motor(int address,signed int duty);
Akito914 0:4346c5764e83 75 void emergencyStop();
Akito914 0:4346c5764e83 76
Akito914 0:4346c5764e83 77 /***** sound *****/
Akito914 0:4346c5764e83 78 void pi(int times);
Akito914 0:4346c5764e83 79 void esc_sound(int key);
Akito914 0:4346c5764e83 80 void q_sound();
Akito914 0:4346c5764e83 81 void ans_sound();
Akito914 0:4346c5764e83 82 void piroro();
Akito914 0:4346c5764e83 83 void beep(int T_us,int t_ms);
Akito914 0:4346c5764e83 84 void beep_freq(int freq, int t_ms);
Akito914 0:4346c5764e83 85 void beep_note(int note, int t_ms);
Akito914 0:4346c5764e83 86 int getPeriod(int freq);
Akito914 0:4346c5764e83 87 /*****************/
Akito914 0:4346c5764e83 88
Akito914 0:4346c5764e83 89 comBuf_t comBuf;
Akito914 0:4346c5764e83 90
Akito914 0:4346c5764e83 91 void reset(){
Akito914 0:4346c5764e83 92 NVIC_SystemReset();
Akito914 0:4346c5764e83 93 }
Akito914 0:4346c5764e83 94
Akito914 0:4346c5764e83 95 bool checksum_check(comBuf_t *buf){
Akito914 0:4346c5764e83 96 int sum = 0;
Akito914 0:4346c5764e83 97
Akito914 0:4346c5764e83 98 for(int count = 0; count < 9; count++){
Akito914 0:4346c5764e83 99 sum += buf->list[count];
Akito914 0:4346c5764e83 100 }
Akito914 0:4346c5764e83 101 return (sum & 0b01111111) == buf->checksum;
Akito914 0:4346c5764e83 102 }
Akito914 0:4346c5764e83 103
Akito914 0:4346c5764e83 104
Akito914 0:4346c5764e83 105 void com_rx()
Akito914 0:4346c5764e83 106 {
Akito914 0:4346c5764e83 107 static int byteCount = 0;
Akito914 0:4346c5764e83 108 char temp = com.getc();
Akito914 0:4346c5764e83 109
Akito914 0:4346c5764e83 110 if(temp == 0b10000000){
Akito914 0:4346c5764e83 111 comBuf.list[0] = temp;
Akito914 0:4346c5764e83 112 byteCount = 1;
Akito914 0:4346c5764e83 113 }
Akito914 0:4346c5764e83 114 else if((temp & 0b10000000) != 0){ // 想定外のデータ
Akito914 0:4346c5764e83 115 byteCount = 0;
Akito914 0:4346c5764e83 116 }
Akito914 0:4346c5764e83 117 else if(byteCount > 0){
Akito914 0:4346c5764e83 118
Akito914 0:4346c5764e83 119 comBuf.list[byteCount] = temp;
Akito914 0:4346c5764e83 120
Akito914 0:4346c5764e83 121 if(byteCount < 9)byteCount += 1;
Akito914 0:4346c5764e83 122 else{ // データは揃った
Akito914 0:4346c5764e83 123
Akito914 0:4346c5764e83 124 if(checksum_check(&comBuf)){
Akito914 0:4346c5764e83 125 led = !led;
Akito914 0:4346c5764e83 126 comTimeout_count = 0;
Akito914 0:4346c5764e83 127 comTimeout_state = false;
Akito914 0:4346c5764e83 128 received_processing(&comBuf);
Akito914 0:4346c5764e83 129 }
Akito914 0:4346c5764e83 130 else{
Akito914 0:4346c5764e83 131 byteCount = 0;
Akito914 0:4346c5764e83 132 }
Akito914 0:4346c5764e83 133 }
Akito914 0:4346c5764e83 134
Akito914 0:4346c5764e83 135 }
Akito914 0:4346c5764e83 136
Akito914 0:4346c5764e83 137 }
Akito914 0:4346c5764e83 138
Akito914 0:4346c5764e83 139 int getJoy7bit(int raw){
Akito914 0:4346c5764e83 140 return raw - 64;
Akito914 0:4346c5764e83 141 }
Akito914 0:4346c5764e83 142
Akito914 0:4346c5764e83 143 void received_processing(comBuf_t *buf){
Akito914 0:4346c5764e83 144
Akito914 0:4346c5764e83 145 if(buf->EMO != 0){
Akito914 0:4346c5764e83 146 ems = true;
Akito914 0:4346c5764e83 147 }
Akito914 0:4346c5764e83 148 else if(ems == true){
Akito914 0:4346c5764e83 149 reset();
Akito914 0:4346c5764e83 150 }
Akito914 0:4346c5764e83 151
Akito914 0:4346c5764e83 152 speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
Akito914 0:4346c5764e83 153 speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
Akito914 0:4346c5764e83 154
Akito914 0:4346c5764e83 155 if(buf->pushL == 1){
Akito914 0:4346c5764e83 156 speed_x *= omuni_burst_coeff;
Akito914 0:4346c5764e83 157 speed_y *= omuni_burst_coeff;
Akito914 0:4346c5764e83 158 }
Akito914 0:4346c5764e83 159
Akito914 0:4346c5764e83 160 if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
Akito914 0:4346c5764e83 161 f = 1;
Akito914 0:4346c5764e83 162 omega = omega_f;
Akito914 0:4346c5764e83 163 if(getJoy7bit(buf->joyRX) > 0)omega *= -1;
Akito914 0:4346c5764e83 164 }
Akito914 0:4346c5764e83 165 else{
Akito914 0:4346c5764e83 166 int diff = (int)buf->L2 - (int)buf->R2;
Akito914 0:4346c5764e83 167 omega = omega_max * diff / 127.0f;
Akito914 0:4346c5764e83 168 f = 0;
Akito914 0:4346c5764e83 169 }
Akito914 0:4346c5764e83 170
Akito914 0:4346c5764e83 171 if(buf->R1 || buf->L1){
Akito914 0:4346c5764e83 172 speed_x = wrap_speed;
Akito914 0:4346c5764e83 173 speed_y = 0;
Akito914 0:4346c5764e83 174 f = 0;
Akito914 0:4346c5764e83 175 omega = wrap_speed / wrap_radius;
Akito914 0:4346c5764e83 176 speed_x *= buf->R1 ? -1 : 1;
Akito914 0:4346c5764e83 177 omega *= buf->R1 ? 1 : -1;
Akito914 0:4346c5764e83 178 }
Akito914 0:4346c5764e83 179
Akito914 0:4346c5764e83 180 arm = buf->arm != 0;
Akito914 0:4346c5764e83 181
Akito914 0:4346c5764e83 182 }
Akito914 0:4346c5764e83 183
Akito914 0:4346c5764e83 184 void comTimeout_intr(){
Akito914 0:4346c5764e83 185
Akito914 0:4346c5764e83 186 if(comTimeout_count >= comTimeout_ms){
Akito914 0:4346c5764e83 187 speed_x = 0;
Akito914 0:4346c5764e83 188 speed_y = 0;
Akito914 0:4346c5764e83 189 omega = 0;
Akito914 0:4346c5764e83 190 arm = 0;
Akito914 0:4346c5764e83 191 comTimeout_state = true;
Akito914 0:4346c5764e83 192 }
Akito914 0:4346c5764e83 193 else{
Akito914 0:4346c5764e83 194 comTimeout_count += 1;
Akito914 0:4346c5764e83 195 }
Akito914 0:4346c5764e83 196
Akito914 0:4346c5764e83 197 }
Akito914 0:4346c5764e83 198
Akito914 0:4346c5764e83 199
Akito914 0:4346c5764e83 200 void arm_control(){
Akito914 0:4346c5764e83 201 char armData[2] = {0};
Akito914 0:4346c5764e83 202
Akito914 0:4346c5764e83 203 armData[0] = arm? armDuty[0] : 0 ;
Akito914 0:4346c5764e83 204 armData[1] = arm? armDuty[1] : 0 ;
Akito914 0:4346c5764e83 205
Akito914 0:4346c5764e83 206 i2cMaster.write(armAddr[0], &armData[0], 1);
Akito914 0:4346c5764e83 207 i2cMaster.write(armAddr[1], &armData[1], 1);
Akito914 0:4346c5764e83 208
Akito914 0:4346c5764e83 209 }
Akito914 0:4346c5764e83 210
Akito914 0:4346c5764e83 211 void control(){
Akito914 0:4346c5764e83 212
Akito914 0:4346c5764e83 213 drive_motor(EMO_Addr, 1);
Akito914 0:4346c5764e83 214
Akito914 0:4346c5764e83 215 omuni.set_speed(speed_x, speed_y, omega, f);
Akito914 0:4346c5764e83 216 omuni.drive();
Akito914 0:4346c5764e83 217
Akito914 0:4346c5764e83 218 arm_control();
Akito914 0:4346c5764e83 219
Akito914 0:4346c5764e83 220 }
Akito914 0:4346c5764e83 221
Akito914 0:4346c5764e83 222 int main()
Akito914 0:4346c5764e83 223 {
Akito914 0:4346c5764e83 224
Akito914 0:4346c5764e83 225 pc.baud(115200);
Akito914 0:4346c5764e83 226 com.baud(115200);
Akito914 0:4346c5764e83 227 pc.printf("Hello!\n");
Akito914 0:4346c5764e83 228 com.attach(com_rx, Serial::RxIrq);
Akito914 0:4346c5764e83 229
Akito914 0:4346c5764e83 230 i2cMaster.frequency(400000);
Akito914 0:4346c5764e83 231
Akito914 0:4346c5764e83 232 omuni.set_speed(0.0f, 0.0f);
Akito914 0:4346c5764e83 233 omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
Akito914 0:4346c5764e83 234 //omuni.set_pid(0, 6.0f, 0.14f, 0.10f);
Akito914 0:4346c5764e83 235 omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
Akito914 0:4346c5764e83 236 omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
Akito914 0:4346c5764e83 237
Akito914 0:4346c5764e83 238 comTimeout.attach_us(comTimeout_intr, 1000);
Akito914 0:4346c5764e83 239
Akito914 0:4346c5764e83 240 led = 0;
Akito914 0:4346c5764e83 241
Akito914 0:4346c5764e83 242 //esc_sound(0);
Akito914 0:4346c5764e83 243 beep_note(96, 100);
Akito914 0:4346c5764e83 244
Akito914 0:4346c5764e83 245 while(1)
Akito914 0:4346c5764e83 246 {
Akito914 0:4346c5764e83 247 wait(0.001);
Akito914 0:4346c5764e83 248
Akito914 0:4346c5764e83 249 if(comTimeout_state == false){
Akito914 0:4346c5764e83 250 control();
Akito914 0:4346c5764e83 251 }
Akito914 0:4346c5764e83 252 else{
Akito914 0:4346c5764e83 253 drive_motor(EMO_Addr, 0);
Akito914 0:4346c5764e83 254 }
Akito914 0:4346c5764e83 255
Akito914 0:4346c5764e83 256 if(ems){
Akito914 0:4346c5764e83 257 emergencyStop();
Akito914 0:4346c5764e83 258 }
Akito914 0:4346c5764e83 259 }
Akito914 0:4346c5764e83 260 }
Akito914 0:4346c5764e83 261
Akito914 0:4346c5764e83 262 int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */
Akito914 0:4346c5764e83 263 char send_data;
Akito914 0:4346c5764e83 264 int ack;
Akito914 0:4346c5764e83 265 if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */
Akito914 0:4346c5764e83 266 send_data=duty;
Akito914 0:4346c5764e83 267 ack=i2cMaster.write(address,&send_data,1);
Akito914 0:4346c5764e83 268 return ack;
Akito914 0:4346c5764e83 269 }
Akito914 0:4346c5764e83 270
Akito914 0:4346c5764e83 271 void emergencyStop(){
Akito914 0:4346c5764e83 272 drive_motor(omuniAddr[0], 0);
Akito914 0:4346c5764e83 273 drive_motor(omuniAddr[1], 0);
Akito914 0:4346c5764e83 274 drive_motor(omuniAddr[2], 0);
Akito914 0:4346c5764e83 275 drive_motor(armAddr[0], 0);
Akito914 0:4346c5764e83 276 drive_motor(armAddr[1], 0);
Akito914 0:4346c5764e83 277
Akito914 0:4346c5764e83 278 drive_motor(EMO_Addr, 0);
Akito914 0:4346c5764e83 279
Akito914 0:4346c5764e83 280 while(1);
Akito914 0:4346c5764e83 281
Akito914 0:4346c5764e83 282 }
Akito914 0:4346c5764e83 283
Akito914 0:4346c5764e83 284
Akito914 0:4346c5764e83 285
Akito914 0:4346c5764e83 286 /*************** sound ***************/
Akito914 0:4346c5764e83 287
Akito914 0:4346c5764e83 288 void pi(int times){
Akito914 0:4346c5764e83 289 int count=0;
Akito914 0:4346c5764e83 290
Akito914 0:4346c5764e83 291 for(count=0;count<times;count++){
Akito914 0:4346c5764e83 292 beep(379,50);
Akito914 0:4346c5764e83 293 wait_ms(50);
Akito914 0:4346c5764e83 294 }
Akito914 0:4346c5764e83 295 wait_ms(300);
Akito914 0:4346c5764e83 296 }
Akito914 0:4346c5764e83 297 /*
Akito914 0:4346c5764e83 298 void esc_sound(void){
Akito914 0:4346c5764e83 299 int count=0;
Akito914 0:4346c5764e83 300
Akito914 0:4346c5764e83 301 wait_ms(60);
Akito914 0:4346c5764e83 302 beep_note(96,150);
Akito914 0:4346c5764e83 303 beep_note(98,150);
Akito914 0:4346c5764e83 304 beep_note(100,220);
Akito914 0:4346c5764e83 305 wait_ms(1200);
Akito914 0:4346c5764e83 306 for(count=0;count<6;count++){
Akito914 0:4346c5764e83 307 beep_note(96,110);
Akito914 0:4346c5764e83 308 wait_ms(150);
Akito914 0:4346c5764e83 309 }
Akito914 0:4346c5764e83 310 wait_ms(1000);
Akito914 0:4346c5764e83 311 beep_note(96,300);
Akito914 0:4346c5764e83 312 wait_ms(100);
Akito914 0:4346c5764e83 313 }*/
Akito914 0:4346c5764e83 314
Akito914 0:4346c5764e83 315 void esc_sound(int key){
Akito914 0:4346c5764e83 316 int count=0;
Akito914 0:4346c5764e83 317
Akito914 0:4346c5764e83 318 wait_ms(60);
Akito914 0:4346c5764e83 319 beep_note(96 + key,150);
Akito914 0:4346c5764e83 320 beep_note(98 + key,150);
Akito914 0:4346c5764e83 321 beep_note(100 + key,220);
Akito914 0:4346c5764e83 322 wait_ms(1200);
Akito914 0:4346c5764e83 323 for(count=0;count<6;count++){
Akito914 0:4346c5764e83 324 beep_note(96 + key,110);
Akito914 0:4346c5764e83 325 wait_ms(150);
Akito914 0:4346c5764e83 326 }
Akito914 0:4346c5764e83 327 wait_ms(1000);
Akito914 0:4346c5764e83 328 beep_note(96 + key,300);
Akito914 0:4346c5764e83 329 wait_ms(100);
Akito914 0:4346c5764e83 330 }
Akito914 0:4346c5764e83 331
Akito914 0:4346c5764e83 332 void q_sound(void){
Akito914 0:4346c5764e83 333 beep(478,100);
Akito914 0:4346c5764e83 334 beep(379,100);
Akito914 0:4346c5764e83 335 }
Akito914 0:4346c5764e83 336
Akito914 0:4346c5764e83 337 void ans_sound(void){
Akito914 0:4346c5764e83 338 beep(379,100);
Akito914 0:4346c5764e83 339 beep(478,100);
Akito914 0:4346c5764e83 340 }
Akito914 0:4346c5764e83 341
Akito914 0:4346c5764e83 342 void piroro(void){
Akito914 0:4346c5764e83 343 beep(379,100);
Akito914 0:4346c5764e83 344 beep(426,100);
Akito914 0:4346c5764e83 345 beep(478,100);
Akito914 0:4346c5764e83 346 }
Akito914 0:4346c5764e83 347
Akito914 0:4346c5764e83 348 void beep(int T_us,int t_ms){
Akito914 0:4346c5764e83 349
Akito914 0:4346c5764e83 350 if(T_us==0 || t_ms==0)return;
Akito914 0:4346c5764e83 351
Akito914 0:4346c5764e83 352 soundOut.period_us(T_us);
Akito914 0:4346c5764e83 353 soundOut.write(0.10);
Akito914 0:4346c5764e83 354
Akito914 0:4346c5764e83 355 wait_ms(t_ms);
Akito914 0:4346c5764e83 356
Akito914 0:4346c5764e83 357 soundOut.write(0.0);
Akito914 0:4346c5764e83 358 soundOut.period_us(100);
Akito914 0:4346c5764e83 359
Akito914 0:4346c5764e83 360 return;
Akito914 0:4346c5764e83 361 }
Akito914 0:4346c5764e83 362
Akito914 0:4346c5764e83 363 void beep_freq(int freq,int t_ms){
Akito914 0:4346c5764e83 364 beep(1000000.0 / freq, t_ms);
Akito914 0:4346c5764e83 365 }
Akito914 0:4346c5764e83 366
Akito914 0:4346c5764e83 367 void beep_note(int note, int t_ms){
Akito914 0:4346c5764e83 368 beep(pow(2.0, (69 - note) / 12.0) * 1000000.0 / 440.0, t_ms);
Akito914 0:4346c5764e83 369 }
Akito914 0:4346c5764e83 370
Akito914 0:4346c5764e83 371 int getPeriod_us(int freq){
Akito914 0:4346c5764e83 372 if(freq<=0)return 0;
Akito914 0:4346c5764e83 373 return 1000000.0/freq ;
Akito914 0:4346c5764e83 374 }
Akito914 0:4346c5764e83 375
Akito914 0:4346c5764e83 376
Akito914 0:4346c5764e83 377
Akito914 0:4346c5764e83 378 /*******************************************/
Akito914 0:4346c5764e83 379