![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
Diff: main.cpp
- Revision:
- 4:5591d3c8a761
- Parent:
- 3:6223efea43fe
- Child:
- 5:12be2ac0f395
--- a/main.cpp Mon Aug 28 07:27:23 2017 +0000 +++ b/main.cpp Mon Sep 11 02:44:27 2017 +0000 @@ -2,15 +2,18 @@ #include "omuni.hpp" #include "cal_PID.hpp" #include "timeBaseTrapezoidalMotionCal.h" +#include "FastPWM.h" +#include "soundData.h" -const float omuni_speed[4] = {0, 1.0, 1.7, 3.0}; +const int comTimeout_ms = 200; +const float omuni_speed[4] = {0, 1.0, 2.0, 3.0}; const float omega_max = 0.7 * 2 * 3.14159265f; const float wrap_radius = 1.0f; -const float wrap_speed = 1.7f; +const float wrap_speed = 1.8f; const int8_t armDuty[] = {-50, -50}; const signed int spearDuty = 127; -const int32_t lift_preset_min = 420; -const int32_t lift_preset_max = 1800; +const int32_t lift_preset_min = 135; +const int32_t lift_preset_max = 450; const int32_t lift_preset[4] = { lift_preset_min, (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, @@ -19,14 +22,17 @@ }; // 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 = 30; -const int32_t lift_down_speed = 30; +const int32_t lift_up_speed = 5; +const int32_t lift_down_speed = 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; int drive_motor(int address,signed int duty); void emergencyStop(); @@ -43,18 +49,25 @@ int past_lift_gray = 0b00; int lift_inc = 0; +const float soundGain = 1.0; +int soundDataCount = 0; +bool soundEnd = false; + //DigitalIn button(USER_BUTTON); DigitalOut led(LED1); -Serial p(USBTX, USBRX); -Serial pc(PA_11, PA_12); +Serial pc(USBTX, USBRX); +Serial com(PA_11, PA_12); 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_6); +Ticker soundRenew; float speed_x, speed_y, omega; bool f; @@ -139,9 +152,9 @@ } -void pc_rx() +void com_rx() { - char temp = pc.getc(); + char temp = com.getc(); comData_t rcTemp; rcTemp.data = temp; float rad, speed; @@ -151,30 +164,31 @@ RCData[rcTemp.byteNum] = rcTemp; /* - p.printf("%d",rcTemp.bit7); - p.printf("%d",rcTemp.bit6); - p.printf("%d",rcTemp.bit5); - p.printf("%d",rcTemp.bit4); - p.printf("%d",rcTemp.bit3); - p.printf("%d",rcTemp.bit2); - p.printf("%d",rcTemp.bit1); - p.printf("%d",rcTemp.bit0); - p.printf("\n"); + pc.printf("%d",rcTemp.bit7); + pc.printf("%d",rcTemp.bit6); + pc.printf("%d",rcTemp.bit5); + pc.printf("%d",rcTemp.bit4); + pc.printf("%d",rcTemp.bit3); + pc.printf("%d",rcTemp.bit2); + pc.printf("%d",rcTemp.bit1); + pc.printf("%d",rcTemp.bit0); + pc.printf("\n"); */ - //p.printf("%d\n", rcTemp.byteNum); - //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); + //pc.printf("%d\n", rcTemp.byteNum); + //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); switch(temp >> 6){ case 0: - //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); + //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); break; case 1: - //p.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys); + //pc.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys); break; case 2: - //p.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down); + //pc.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down); break; case 3: + break; } @@ -182,6 +196,7 @@ if(rcTemp.byteNum == 3){ // データは揃った led = !led; + comTimeout_count = 0; ems = RCData[3].emo != 0; @@ -237,7 +252,7 @@ } if(lift_diff != 0){ if(lift_diff == 1){ - if(lift_preset[lift_nearest_num] >= lift_currentTarg){p.printf("$$"); + if(lift_preset[lift_nearest_num] >= lift_currentTarg){ if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; } else{ @@ -261,8 +276,8 @@ } past_lift_gray = RCData[2].lift_gray; - if(RCData[2].lift_down != 0)lift_inc = -2; - else if(RCData[2].lift_up != 0)lift_inc = 2; + if(RCData[2].lift_down != 0)lift_inc = -1; + else if(RCData[2].lift_up != 0)lift_inc = 1; else lift_inc = 0; } // すっきりした // してない @@ -270,6 +285,39 @@ } +void soundRenew_intr(){ + + if(soundDataCount < soundData_size - 1000){ + + soundOut.write((soundData[soundDataCount] * soundGain / 256.0) + 0.5); + + soundDataCount++; + + } + else{ + soundEnd = true; + } + +} + + +void comTimeout_intr(){ + + if(comTimeout_count >= comTimeout_ms){ + speed_x = 0; + speed_y = 0; + omega = 0; + arm = 0; + spear = 0; + lift_inc = 0; + } + else{ + comTimeout_count += 1; + } + +} + + void arm_control(){ char armData[2] = {0}; @@ -308,14 +356,17 @@ } } - meter = lift_meter.read_u16() >> 4; + meter = lift_meter.read_u16() >> 6; 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 = 500; static int spear_state = 0; + static int spear_timeout_count = 0; signed int duty = 0; int sensor = spear_sensor; // リミット時に1 @@ -332,15 +383,33 @@ break; case 1 : // 初期位置から抜け出す duty = spearDuty; - if(sensor == 0){ + 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; @@ -378,27 +447,37 @@ int period_us = 0; int count = 0; - p.baud(115200); - pc.baud(9600); - p.printf("Hello!\n"); - pc.attach(pc_rx, Serial::RxIrq); + pc.baud(115200); + com.baud(9600); + pc.printf("Hello!\n"); + com.attach(com_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(0, 6.0f, 0.14f, 0.10f); omuni.set_pid(1, 3.0f, 0.07f, 0.05f); omuni.set_pid(2, 3.0f, 0.07f, 0.05f); - lift_pid.param(0.4, 0.0, 0.0); + lift_pid.param(1.5, 0.0, 0.0); lift_pid.period(0.001); lift_pid.output(-127, 127); - //control_loop.attach_us(&control, 1000); + comTimeout.attach_us(comTimeout_intr, 1000); led = 0; - //mesPeriod.start(); + /* + soundOut.period_us(10); + for(count = 0; count < 500; count++){ + wait_ms(1); + soundOut.write(count / 100.0); + } + soundRenew.attach_us(soundRenew_intr, 63); + + while(soundEnd == false); + */ while(1) { @@ -407,6 +486,8 @@ omuni.drive(); control(); + drive_motor(EMO_Addr, 1); + if(ems){ emergencyStop(); } @@ -422,7 +503,6 @@ return ack; } - void emergencyStop(){ drive_motor(omuniAddr[0], 0); drive_motor(omuniAddr[1], 0); @@ -431,6 +511,9 @@ drive_motor(armAddr[1], 0); drive_motor(liftAddr, 0); drive_motor(spearAddr, 0); + + drive_motor(EMO_Addr, 0); + while(1); }