GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
Diff: main.cpp
- Revision:
- 10:f17b33dd837d
- Parent:
- 9:bf6d20813364
--- a/main.cpp Thu Sep 28 10:20:53 2017 +0000 +++ b/main.cpp Mon Oct 09 02:29:36 2017 +0000 @@ -7,27 +7,26 @@ #define LIFT_METER_INVERSION 1 const int comTimeout_ms = 200; -const float omuni_speed_max = 3.0f; -const float omuni_burst_coeff = 1.8f; -const float omega_max = 1.2 * 2 * 3.14159265f; -const float omega_f = 0.5 * 2 * 3.14159265f; -const float wrap_radius = 1.0f; -const float wrap_speed = 2.2f; +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 = 250; -//const int32_t lift_preset_min = 320; // Temporary use -const int32_t lift_preset_max = 480; +const int32_t lift_preset_min = 150; +const int32_t lift_preset_max = 410; // + 260 const int32_t lift_preset[4] = { lift_preset_min, - (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, - (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + 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 = 10; -const int32_t lift_down_speed = 10; +const int32_t lift_up_speed = 50; +const int32_t lift_down_speed = 50; const int32_t lift_prescaler = 5; @@ -41,7 +40,7 @@ bool comTimeout_state = true; -bool ems = false; +volatile bool ems = false; bool recovery = false; bool arm = false; @@ -62,7 +61,10 @@ DigitalOut led(LED1); Serial pc(USBTX, USBRX); -Serial com(PA_11, PA_12); +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); @@ -126,7 +128,9 @@ /*****************/ -comBuf_t comBuf; +comBuf_t comMainBuf; +comBuf_t comSubBuf; + void reset(){ NVIC_SystemReset(); @@ -173,13 +177,13 @@ } -void com_rx() +void comMain_rx() { static int byteCount = 0; - char temp = com.getc(); + char temp = comMain.getc(); if(temp == 0b10000000){ - comBuf.list[0] = temp; + comMainBuf.list[0] = temp; byteCount = 1; } else if((temp & 0b10000000) != 0){ // 想定外のデータ @@ -187,16 +191,50 @@ } else if(byteCount > 0){ - comBuf.list[byteCount] = temp; + comMainBuf.list[byteCount] = temp; if(byteCount < 9)byteCount += 1; else{ // データは揃った - if(checksum_check(&comBuf)){ + if(checksum_check(&comMainBuf)){ led = !led; comTimeout_count = 0; comTimeout_state = false; - received_processing(&comBuf); + 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; @@ -214,6 +252,11 @@ 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; @@ -231,8 +274,22 @@ } 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; @@ -352,6 +409,7 @@ comTimeout_count += 1; } + } @@ -412,7 +470,7 @@ void spear_control(){ const int spear_timeout_short = 100; - const int spear_timeout_long = 500; + const int spear_timeout_long = 200; static int spear_state = 0; static int spear_timeout_count = 0; signed int duty = 0; @@ -497,10 +555,15 @@ int main() { + bool ex_comTimeout_state = true; + char currentState; + pc.baud(115200); - com.baud(115200); + comMain.baud(19200); + comSub.baud(115200); pc.printf("Hello!\n"); - com.attach(com_rx, Serial::RxIrq); + comMain.attach(comMain_rx, Serial::RxIrq); + comSub.attach(comSub_rx, Serial::RxIrq); i2cMaster.frequency(400000); @@ -509,7 +572,12 @@ omuni.set_pid(1, 3.0f, 0.07f, 0.05f); omuni.set_pid(2, 3.0f, 0.07f, 0.05f); - lift_pid.param(1.5, 0.0, 0.0); + 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); @@ -525,10 +593,34 @@ */ 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(); } @@ -536,6 +628,17 @@ 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(); } @@ -562,7 +665,11 @@ drive_motor(EMO_Addr, 0); - while(1); + beep_note(100, 100); + beep_note(96, 100); + + while(1){ + } } @@ -579,23 +686,6 @@ } wait_ms(300); } -/* -void esc_sound(void){ - int count=0; - - wait_ms(60); - beep_note(96,150); - beep_note(98,150); - beep_note(100,220); - wait_ms(1200); - for(count=0;count<6;count++){ - beep_note(96,110); - wait_ms(150); - } - wait_ms(1000); - beep_note(96,300); - wait_ms(100); -}*/ void esc_sound(int key){ int count=0;