![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
main.cpp
- Committer:
- Akito914
- Date:
- 2017-10-09
- Revision:
- 10:f17b33dd837d
- Parent:
- 9:bf6d20813364
File content as of revision 10:f17b33dd837d:
#include "mbed.h" #include "omuni.hpp" #include "cal_PID.hpp" #include "FastPWM.h" #include "sound_data.h" #define LIFT_METER_INVERSION 1 const int comTimeout_ms = 200; const float omuni_speed_max = 4.5f; const float omuni_burst_coeff = 1.5f; const float omega_max = 1.5 * 2 * 3.14159265f; const float omega_f = 1.0 * 2 * 3.14159265f; const float wrap_radius = 1.5f; const float wrap_speed = 3.5f; const int8_t armDuty[] = {-100, -100}; const signed int spearDuty = 127; const int32_t lift_preset_min = 150; const int32_t lift_preset_max = 410; // + 260 const int32_t lift_preset[4] = { lift_preset_min, (int32_t)((lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min), (int32_t)((lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min), lift_preset_max, }; // 12bit 0 ~ 4095 const int32_t lift_max = lift_preset_max; const int32_t lift_min = lift_preset_min; const int32_t lift_up_speed = 50; const int32_t lift_down_speed = 50; const int32_t lift_prescaler = 5; const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 const int spearAddr = 0x1a; // 1010 const int liftAddr = 0x1c; // 0110 const int EMO_Addr = 0x1e; // 1110 int comTimeout_count = 0; bool comTimeout_state = true; volatile bool ems = false; bool recovery = false; bool arm = false; bool spear = false; bool check_beep = false; int lift_targ = lift_preset[0]; int lift_stepTarg; bool lift_stepMoving = false; bool first_receive = true; int past_lift_gray = 0b00; int lift_inc = 0; int sound_count = 0; DigitalIn button(USER_BUTTON); DigitalOut led(LED1); Serial pc(USBTX, USBRX); Serial comMain(PA_11, PA_12); Serial comSub(PB_6, PA_10); I2C i2cMaster(D14, D15); Omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f); AnalogIn lift_meter(PC_0); DigitalIn spear_sensor(PC_3); Timer mesPeriod; cal_pid lift_pid; Ticker comTimeout; FastPWM soundOut(PB_7); float speed_x, speed_y, omega; bool f; typedef union{ struct{ unsigned startByte :8; unsigned joyLX :8; unsigned joyLY :8; unsigned joyRX :8; unsigned joyRY :8; unsigned L2 :8; unsigned R2 :8; unsigned arrowL :1; unsigned arrowR :1; unsigned arrowU :1; unsigned arrowD :1; unsigned L1 :1; unsigned R1 :1; unsigned SELECT :2; unsigned lift_gray :2; unsigned spear :1; unsigned arm :1; unsigned pushL :1; unsigned pushR :1; unsigned EMO :2; unsigned checksum :8; }; uint8_t list[10]; }comBuf_t; void received_processing(comBuf_t *buf); int byteSum(int8_t byte); bool checksum_check(comBuf_t *buf); int drive_motor(int address,signed int duty); void emergencyStop(); /***** sound *****/ void pi(int times); void esc_sound(int key); void q_sound(); void ans_sound(); void piroro(); void beep(int T_us,int t_ms); void beep_freq(int freq, int t_ms); void beep_note(int note, int t_ms); void check_beep_turn(bool turn); int getPeriod(int freq); int freq2period_us(int freq); int note2period_us(int note); /*****************/ comBuf_t comMainBuf; comBuf_t comSubBuf; void reset(){ NVIC_SystemReset(); } int grayDiffer2bit(int gray, int origin){ switch(origin){ case 0b00: switch(gray){ case 0b01: return 1; case 0b10: return -1; default : return 0; } case 0b01: switch(gray){ case 0b11: return 1; case 0b00: return -1; default : return 0; } case 0b11: switch(gray){ case 0b10: return 1; case 0b01: return -1; default : return 0; } case 0b10: switch(gray){ case 0b00: return 1; case 0b11: return -1; default : return 0; } } return 0; } bool checksum_check(comBuf_t *buf){ int sum = 0; for(int count = 0; count < 9; count++){ sum += buf->list[count]; } return (sum & 0b01111111) == buf->checksum; } void comMain_rx() { static int byteCount = 0; char temp = comMain.getc(); if(temp == 0b10000000){ comMainBuf.list[0] = temp; byteCount = 1; } else if((temp & 0b10000000) != 0){ // 想定外のデータ byteCount = 0; } else if(byteCount > 0){ comMainBuf.list[byteCount] = temp; if(byteCount < 9)byteCount += 1; else{ // データは揃った if(checksum_check(&comMainBuf)){ led = !led; comTimeout_count = 0; comTimeout_state = false; received_processing(&comMainBuf); } else{ byteCount = 0; } } } } void comSub_rx() { static int byteCount = 0; char temp = comSub.getc(); if(temp == 0b10000000){ comSubBuf.list[0] = temp; byteCount = 1; } else if((temp & 0b10000000) != 0){ // 想定外のデータ byteCount = 0; } else if(byteCount > 0){ comSubBuf.list[byteCount] = temp; if(byteCount < 9)byteCount += 1; else{ // データは揃った if(checksum_check(&comSubBuf)){ led = !led; comTimeout_count = 0; comTimeout_state = false; received_processing(&comSubBuf); } else{ byteCount = 0; } } } } int getJoy7bit(int raw){ return raw - 64; } void received_processing(comBuf_t *buf){ int lift_nearest_num = 0; float speed = 0; float raw_speed_x, raw_speed_y; float degree; int angle_num; const int omuni_angle_split_num = 8; if(buf->EMO != 0){ ems = true; } else if(ems == true){ reset(); } if(check_beep == false && buf->SELECT == true){ check_beep_turn(true); } else if(check_beep == true && buf->SELECT == false){ check_beep_turn(false); } check_beep = buf->SELECT; raw_speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f; raw_speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f; speed = sqrt(raw_speed_x * raw_speed_x + raw_speed_y * raw_speed_y); degree = atan2((double)raw_speed_y, (double)raw_speed_x) * 180.0 / 3.14159265358979; angle_num = (double)degree * omuni_angle_split_num / 360.0 + ((degree < 0)?-0.5:0.5); speed_x = speed * cos(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num); speed_y = speed * sin(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num); /* speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f; speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f; */ if(buf->pushL == 1){ speed_x *= omuni_burst_coeff; speed_y *= omuni_burst_coeff; } if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){ f = 1; omega = omega_f; if(getJoy7bit(buf->joyRX) > 0)omega *= -1; } else{ int diff = (int)buf->L2 - (int)buf->R2; omega = omega_max * diff / 127.0f; f = 0; } if(buf->R1 || buf->L1){ float speed_wrap_x; float speed_wrap_y; float omega_wrap; speed_wrap_x = wrap_speed; speed_wrap_y = 0; omega_wrap = wrap_speed / wrap_radius; speed_wrap_x *= buf->R1 ? -1 : 1; omega_wrap *= buf->R1 ? 1 : -1; if(f == 1){ speed_x = 0; speed_y = 0; omega = 0; } speed_x += speed_wrap_x; speed_y += speed_wrap_y; omega += omega_wrap; } arm = buf->arm != 0; spear = buf->spear != 0; if(first_receive){ past_lift_gray = buf->lift_gray; first_receive = false; } if(buf->lift_gray != past_lift_gray){ // 段階移動 int lift_currentTarg; int lift_diff = 0; int lift_nearest = 10000; int lift_nextTarg; lift_currentTarg = lift_targ; if(lift_stepMoving){ lift_nextTarg = lift_stepTarg; } else{ lift_nextTarg = lift_currentTarg; } lift_diff = grayDiffer2bit(buf->lift_gray, past_lift_gray); for(int num = 0; num < 4; num++){ // 最寄りの段階 if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){ lift_nearest_num = num; lift_nearest = abs(lift_preset[num] - lift_currentTarg); } } if(lift_diff != 0){ if(lift_diff == 1){ if(lift_preset[lift_nearest_num] <= lift_currentTarg){ if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; } else{ lift_nextTarg = lift_preset[lift_nearest_num]; } } else if(lift_diff == -1){ if(lift_preset[lift_nearest_num] >= lift_currentTarg){ if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; } else{ lift_nextTarg = lift_preset[lift_nearest_num]; } } lift_stepTarg = lift_nextTarg; lift_stepMoving = true; } } past_lift_gray = buf->lift_gray; if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){ if(getJoy7bit(buf->joyRY) > 0)lift_inc = -5; else lift_inc = 5; } else{ lift_inc = 0; } } void comTimeout_intr(){ if(comTimeout_count >= comTimeout_ms){ speed_x = 0; speed_y = 0; omega = 0; arm = 0; spear = 0; lift_inc = 0; comTimeout_state = true; } else{ comTimeout_count += 1; } } void arm_control(){ char armData[2] = {0}; armData[0] = arm? armDuty[0] : 0 ; armData[1] = arm? armDuty[1] : 0 ; i2cMaster.write(armAddr[0], &armData[0], 1); i2cMaster.write(armAddr[1], &armData[1], 1); } void lift_control(){ static int prescaler_count = 0; int32_t meter = 0; signed int duty = 0; int32_t error = 0; if(prescaler_count >= lift_prescaler){ prescaler_count = 0; error = lift_stepTarg - lift_targ; if(lift_stepMoving){ if(error > lift_up_speed){ lift_targ += lift_up_speed; } else if(error < lift_down_speed * -1){ lift_targ -= lift_down_speed; } else{ lift_targ = lift_stepTarg; lift_stepMoving = false; } } else{ if(lift_inc < 0){ if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; } else if(lift_inc > 0){ if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc; } } } else{ prescaler_count += 1; } meter = lift_meter.read_u16() >> 6; #if LIFT_METER_INVERSION meter = 1023 - meter; #endif duty = lift_pid.get_pid(meter, lift_targ, 1); drive_motor(liftAddr, duty); } void spear_control(){ const int spear_timeout_short = 100; const int spear_timeout_long = 200; static int spear_state = 0; static int spear_timeout_count = 0; signed int duty = 0; int sensor = spear_sensor; // リミット時に1 switch(spear_state){ case 0 : // 待ち if(spear){ spear_state = 1; } else{ if(sensor == 0){ duty = spearDuty * -1; } } break; case 1 : // 初期位置から抜け出す duty = spearDuty; if(sensor != 0){ if(spear_timeout_count < spear_timeout_short){ spear_timeout_count += 1; } else{ spear_timeout_count = 0; spear_state = 0; } } else{ spear_timeout_count = 0; spear_state = 2; } break; case 2 : // 到達位置まで動かす if(sensor == 0){ duty = spearDuty; if(spear_timeout_count < spear_timeout_long){ spear_timeout_count += 1; } else{ spear_timeout_count = 0; spear_state = 3; } } else{ spear_timeout_count = 0; spear_state = 3; } break; case 3 : // 到達位置から抜け出す duty = spearDuty * -1; if(sensor == 0){ spear_state = 4; } break; case 4 : // 初期位置まで戻る if(sensor == 0){ duty = spearDuty * -1; } else{ spear_state = 0; } break; default : break; } drive_motor(spearAddr, duty); } void control(){ drive_motor(EMO_Addr, 1); omuni.set_speed(speed_x, speed_y, omega, f); omuni.drive(); arm_control(); lift_control(); spear_control(); } int main() { bool ex_comTimeout_state = true; char currentState; pc.baud(115200); comMain.baud(19200); comSub.baud(115200); pc.printf("Hello!\n"); comMain.attach(comMain_rx, Serial::RxIrq); comSub.attach(comSub_rx, Serial::RxIrq); i2cMaster.frequency(400000); omuni.set_speed(0.0f, 0.0f); omuni.set_pid(0, 3.0f, 0.07f, 0.05f); omuni.set_pid(1, 3.0f, 0.07f, 0.05f); omuni.set_pid(2, 3.0f, 0.07f, 0.05f); omuni.change_motor_direction(0, false); omuni.change_motor_direction(1, false); omuni.change_motor_direction(2, false); omuni.change_coordinates_direction(true, true); lift_pid.param(1.0, 0.0, 0.0); lift_pid.period(0.001); lift_pid.output(-127, 127); comTimeout.attach_us(comTimeout_intr, 1000); led = 0; //esc_sound(0); /* beep_note(96, 150); beep_note(98, 150); beep_note(100, 220); */ beep_note(100, 500); int initial_meter = lift_meter.read_u16() >> 6; #if LIFT_METER_INVERSION initial_meter = 1023 - initial_meter; #endif lift_targ = initial_meter; lift_stepTarg = lift_preset[0]; lift_stepMoving = true; drive_motor(EMO_Addr, 3); while(1) { wait(0.001); i2cMaster.read(EMO_Addr, ¤tState, 1); if((currentState & 0b00000010) != 0){ for(int count = 0; count < 15; count++){ beep_note(100, 100); wait_ms(100); } while(ems == false); emergencyStop(); } if(comTimeout_state == false){ control(); } else{ drive_motor(EMO_Addr, 0); } if(ex_comTimeout_state == false && comTimeout_state == true){ beep_note(100, 100); beep_note(96, 100); } else if(ex_comTimeout_state == true && comTimeout_state == false){ beep_note(96, 100); beep_note(100, 100); } ex_comTimeout_state = comTimeout_state; if(ems){ emergencyStop(); } } } int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */ char send_data; int ack; if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */ send_data=duty; ack=i2cMaster.write(address,&send_data,1); return ack; } void emergencyStop(){ drive_motor(omuniAddr[0], 0); drive_motor(omuniAddr[1], 0); drive_motor(omuniAddr[2], 0); drive_motor(armAddr[0], 0); drive_motor(armAddr[1], 0); drive_motor(liftAddr, 0); drive_motor(spearAddr, 0); drive_motor(EMO_Addr, 0); beep_note(100, 100); beep_note(96, 100); while(1){ } } /*************** sound ***************/ void pi(int times){ int count=0; for(count=0;count<times;count++){ beep(379,50); wait_ms(50); } wait_ms(300); } void esc_sound(int key){ int count=0; wait_ms(60); beep_note(96 + key,150); beep_note(98 + key,150); beep_note(100 + key,220); wait_ms(1200); for(count=0;count<6;count++){ beep_note(96 + key,110); wait_ms(150); } wait_ms(1000); beep_note(96 + key,300); wait_ms(100); } void q_sound(void){ beep(478,100); beep(379,100); } void ans_sound(void){ beep(379,100); beep(478,100); } void piroro(void){ beep(379,100); beep(426,100); beep(478,100); } void beep(int T_us,int t_ms){ if(T_us==0 || t_ms==0)return; soundOut.period_us(T_us); soundOut.write(0.50); wait_ms(t_ms); soundOut.write(0.0); soundOut.period_us(100); return; } void beep_freq(int freq,int t_ms){ beep(1000000.0 / freq, t_ms); } void beep_note(int note, int t_ms){ beep(pow(2.0, (69 - note) / 12.0) * 1000000.0 / 440.0, t_ms); } void check_beep_turn(bool turn){ if(turn){ soundOut.period_us(note2period_us(note_kouka[sound_count] + kouka_key)); soundOut.write(0.50); if(sound_count < kouka_size - 1)sound_count++; else sound_count = 0; } else{ soundOut.write(0.0); } } int getPeriod_us(int freq){ if(freq<=0)return 0; return 1000000.0/freq ; } int freq2period_us(int freq){ if(freq<=0)return 0; return 1000000.0/freq ; } int note2period_us(int note){ return freq2period_us(440.0 * pow(2.0, (note - 69) / 12.0)); } /*******************************************/