GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
Diff: main.cpp
- Revision:
- 1:baab5b88a142
- Parent:
- 0:6da7d0e457a2
- Child:
- 2:c9de02d6d154
diff -r 6da7d0e457a2 -r baab5b88a142 main.cpp --- a/main.cpp Thu Aug 17 03:49:34 2017 +0000 +++ b/main.cpp Wed Aug 23 07:50:14 2017 +0000 @@ -1,96 +1,405 @@ #include "mbed.h" #include "omuni.hpp" +#include "cal_PID.hpp" -int addr[] = {0x10, 0x12, 0x14}; -int armAddr[] = {0x16, 0x18}; +const float omega_max = 1.0 * 2 * 3.14159265f; +const float wrap_radius = 1.5f; +const float wrap_speed = 2.0f; +const int8_t armDuty[] = {-50, -50}; +const signed int spearDuty = 127; +const int32_t lift_preset[4] = {620, 990, 1360, 1730}; // 12bit 0 ~ 4095 +const int32_t lift_max = 1830; +const int32_t lift_min = 500; + +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 + +int drive_motor(int address,signed int duty); +void emergencyStop(); + +bool ems = false; + bool arm = false; +bool spear = false; +int lift_targ = lift_preset[0]; +bool first_receive = true; +int past_lift_gray = 0b00; +int lift_inc = 0; + +//DigitalIn button(USER_BUTTON); +DigitalOut led(LED1); Serial p(USBTX, USBRX); Serial pc(PA_11, PA_12); I2C i2cMaster(D14, D15); -// archan -omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f); +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; + +float speed_x, speed_y, omega; +bool f; -float vx, vy, ome; -bool f, pre_f; -char recv[3] = {0}; +typedef union{ + struct{ + unsigned angle :4; + unsigned speed :2; + unsigned byteNum0:2; + }; + struct{ + unsigned wrap_R :1; + unsigned wrap_L :1; + unsigned coorSys :1; + unsigned omega :2; + unsigned dir :1; + unsigned byteNum1 :2; + }; + struct{ + unsigned lift_down :1; + unsigned lift_up :1; + unsigned lift_gray :2; + unsigned spear :1; + unsigned arm :1; + unsigned byteNum2 :2; + }; + struct{ + unsigned reserved2 :1; + unsigned emo :1; + unsigned reserved3 :4; + unsigned byteNum3 :2; + }; + struct{ + unsigned control :6; + unsigned byteNum :2; + }; + struct{ + unsigned bit0:1; + unsigned bit1:1; + unsigned bit2:1; + unsigned bit3:1; + unsigned bit4:1; + unsigned bit5:1; + unsigned bit6:1; + unsigned bit7:1; + }; + int8_t data; +}comData_t; + + +comData_t RCData[4] = {0}; + +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; +} + void pc_rx() { - char rtemp = pc.getc(); + char temp = pc.getc(); + comData_t rcTemp; + rcTemp.data = temp; + float rad, speed; + int lift_nearest_num = 0; - if((rtemp & 0b11000000) == 0b00000000) recv[0] = rtemp; - else if((rtemp & 0b11000000) == 0b01000000) recv[1] = rtemp; - else if((rtemp & 0b11000000) == 0b10000000) recv[2] = rtemp; + + RCData[rcTemp.byteNum] = rcTemp; - float direc = (recv[0] & 0x0f) * 3.141592 / 8.0; - float speed = 0.8 * ((recv[0] & 0b00110000) >> 4); - vx = speed * cos(direc) * -1; - vy = speed * sin(direc) * -1; + /* + 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"); + */ + //p.printf("%d\n", rcTemp.byteNum); + //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); - if(recv[2] & 0b1) - { - arm = true; - } - else - { - arm = false; + switch(temp >> 6){ + case 0: + //p.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); + 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); + break; + case 3: + + break; } - if(recv[1] & 0b11000) - { - f = false; - ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0; - if(recv[1] & 0b100000) ome *= -1; + if(rcTemp.byteNum == 3){ // データは揃った + + led = !led; + + ems = RCData[3].emo != 0; + + rad = RCData[0].angle * 3.141592 / 8.0; + speed = 0.8 * RCData[0].speed; + speed_x = speed * cos(rad) * -1; + speed_y = speed * sin(rad) * -1; + + f = RCData[1].coorSys; + omega = RCData[1].omega * omega_max / 3.0f; + omega *= RCData[1].dir ? 1 : -1; + + + if(RCData[1].wrap_R || RCData[1].wrap_L){ + speed_x = wrap_speed; + speed_y = 0; + f = 0; + omega = wrap_speed / wrap_radius; + speed_x *= RCData[1].wrap_L ? 1 : -1; + omega *= RCData[1].wrap_L ? 1 : -1; + } + + + arm = RCData[2].arm != 0; + + spear = RCData[2].spear != 0; + + if(first_receive){ + past_lift_gray = RCData[2].lift_gray; + first_receive = false; + } + if(RCData[2].lift_gray != past_lift_gray){ // 段階移動 + int lift_diff = 0; + int lift_nearest = 10000; + lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray); + //p.printf("%d --> %d\n", past_lift_gray, RCData[2].lift_gray); + //p.printf("gray = %d\n", lift_diff); + for(int num = 0; num < 4; num++){ // 最寄りの段階 + //p.printf("nearest = %d, num = %d\n", lift_nearest, lift_nearest_num); + if(lift_nearest > abs(lift_preset[num] - lift_targ)){ + lift_nearest_num = num; + lift_nearest = abs(lift_preset[num] - lift_targ); + } + } + //p.printf("lift_targ = %d num = %d\n", lift_targ, lift_nearest_num); + if(lift_diff == 1){ + if(lift_preset[lift_nearest_num] >= lift_targ){ + if(lift_nearest_num < 3)lift_targ = lift_preset[lift_nearest_num + 1]; + } + else{ + lift_targ = lift_preset[lift_nearest_num]; + } + } + else if(lift_diff == -1){ + if(lift_preset[lift_nearest_num] <= lift_targ){ + if(lift_nearest_num > 0)lift_targ = lift_preset[lift_nearest_num - 1]; + } + else{ + lift_targ = lift_preset[lift_nearest_num]; + } + } + } + past_lift_gray = RCData[2].lift_gray; + + if(RCData[2].lift_down != 0)lift_inc = -5; + else if(RCData[2].lift_up != 0)lift_inc = 5; + else lift_inc = 0; + + //p.printf("inc = %d\n",lift_inc); + //p.printf("lift_targ = %d\n", lift_targ); + + } // すっきりした // してない + +} + + +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(){ + int32_t meter = 0; + signed int duty = 0; + + if(lift_inc < 0){ + if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; } - else - { - if(recv[1] & 0b11) - { - f = true; -// if(pre_f == false) omuni.reset_theta(); - } - else - { - f = false; - } - ome = (recv[1] & 0b11) * 3.141592 / 2.0; - if((recv[1] & 0b100)) ome *= -1; + else if(lift_inc > 0){ + if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc; } - pre_f = f; + meter = lift_meter.read_u16() >> 4; + duty = lift_pid.get_pid(meter, lift_targ); + drive_motor(liftAddr, duty); + +} + +void spear_control(){ + static int spear_state = 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){ + spear_state = 2; + } + break; + case 2 : // 到達位置まで動かす + if(sensor == 0){ + duty = spearDuty; + } + else{ + 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(){ + + arm_control(); + lift_control(); + spear_control(); + } int main() { + int period_us = 0; + int count = 0; + p.baud(115200); pc.baud(9600); - pc.printf("Hello!\n"); + p.printf("Hello!\n"); pc.attach(pc_rx, Serial::RxIrq); + + //i2cMaster.frequency(400000); + omuni.set_speed(0.0f, 0.0f); - // archan 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); + lift_pid.param(0.3, 0.0, 0.0); + lift_pid.period(0.001); + lift_pid.output(-127, 127); + + //control_loop.attach_us(&control, 1000); + + led = 0; + + //mesPeriod.start(); + while(1) { - wait(0.001); - omuni.set_speed(vx, vy, ome, f); + wait(0.0001); + omuni.set_speed(speed_x, speed_y, omega, f); omuni.drive(); + control(); - if(arm) - { - char send = -127; - i2cMaster.write(armAddr[0], &send, 1); - i2cMaster.write(armAddr[1], &send, 1); + if(ems){ + emergencyStop(); + } + /* + if(count < 100){ + count += 1; + } + else{ + period_us = mesPeriod.read_us(); + mesPeriod.reset(); + + p.printf("%dHz\n", 1000000 * 100 / period_us); + + count = 0; } - else - { - char send = 0; - i2cMaster.write(armAddr[0], &send, 1); - i2cMaster.write(armAddr[1], &send, 1); - } - - p.printf("%f\n", omuni.get_theta()); + */ } -} \ No newline at end of file +} + +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); + while(1); +} + +