![](/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:
- 5:12be2ac0f395
- Parent:
- 4:5591d3c8a761
- Child:
- 6:22ad12682a5d
diff -r 5591d3c8a761 -r 12be2ac0f395 main.cpp --- a/main.cpp Mon Sep 11 02:44:27 2017 +0000 +++ b/main.cpp Mon Sep 11 05:37:59 2017 +0000 @@ -7,7 +7,9 @@ const int comTimeout_ms = 200; const float omuni_speed[4] = {0, 1.0, 2.0, 3.0}; +const float omuni_speed_max = 2.0f; const float omega_max = 0.7 * 2 * 3.14159265f; +const float omega_f = 0.7 * 2 * 3.14159265f; const float wrap_radius = 1.0f; const float wrap_speed = 1.8f; const int8_t armDuty[] = {-50, -50}; @@ -34,8 +36,7 @@ int comTimeout_count = 0; -int drive_motor(int address,signed int duty); -void emergencyStop(); + bool ems = false; @@ -74,51 +75,40 @@ 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; + 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; }; - 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; + + 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(); -comData_t RCData[4] = {0}; +comBuf_t comBuf; + int grayDiffer2bit(int gray, int origin){ @@ -151,136 +141,155 @@ return 0; } +int byteSum(int8_t byte){ + int sum = 0; + for(int count = 0; count < 8; count++){ + if((byte & (0x01 << count)) != 0)sum += 1; + } + return sum; +} + +bool checksum_check(comBuf_t *buf){ + int sum = 0; + + for(int count = 0; count < 9; count++){ + sum += byteSum(buf->list[count]); + } + return sum == buf->checksum; +} + void com_rx() { + static int byteCount = 0; char temp = com.getc(); - comData_t rcTemp; - rcTemp.data = temp; - float rad, speed; + + if(temp == 0b10000000){ + comBuf.list[0] = temp; + byteCount = 1; + } + else if(byteCount & 0b10000000 != 0){ // 想定外のデータ + byteCount = 0; + } + else if(byteCount > 0){ + + comBuf.list[byteCount] = temp; + + if(byteCount < 9)byteCount += 1; + else{ // データは揃った + if(checksum_check(&comBuf)){ + led = !led; + comTimeout_count = 0; + received_processing(&comBuf); + } + else{ + byteCount = 0; + } + } + + } + +} + +int getJoy7bit(int raw){ + return raw - 64; +} + +void received_processing(comBuf_t *buf){ + int lift_nearest_num = 0; + ems = buf->EMO != 0; - RCData[rcTemp.byteNum] = rcTemp; + speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f; + speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f; - /* - 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"); - */ - //pc.printf("%d\n", rcTemp.byteNum); - //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); + if(abs(getJoy7bit(buf->joyRY)) < 10 && abs(getJoy7bit(buf->joyRX)) > 40){ + f = 1; + omega = omega_f; + if(getJoy7bit(buf->joyRX) < 0)omega *= 1; + } - switch(temp >> 6){ - case 0: - //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); - break; - case 1: - //pc.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys); - break; - case 2: - //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; + if(buf->L2 >= 5 && buf->R2 < 5){ + omega = omega_max * buf->L2 / 127.0f; + f = 0; + } + else if(buf->L2 < 5 && buf->R2 >= 5){ + omega = omega_max * buf->L2 * -1.0f / 127.0f; + f = 0; } - if(rcTemp.byteNum == 3){ // データは揃った - - led = !led; - comTimeout_count = 0; - - ems = RCData[3].emo != 0; - - rad = RCData[0].angle * 3.141592 / 8.0; - speed = omuni_speed[RCData[0].speed]; - if(RCData[0].speed == 0)speed = 0; - speed_x = speed * cos(rad) * -1; - speed_y = speed * sin(rad) * -1; + if(buf->R1 || buf->L1){ + speed_x = wrap_speed; + speed_y = 0; + f = 0; + omega = wrap_speed / wrap_radius; + speed_x *= buf->L1 ? 1 : -1; + omega *= buf->L1 ? 1 : -1; + } + + 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; - 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; + lift_currentTarg = lift_targ; + if(lift_stepMoving){ + lift_nextTarg = lift_stepTarg; + } + else{ + lift_nextTarg = lift_currentTarg; } - - arm = RCData[2].arm != 0; - - spear = RCData[2].spear != 0; - - if(first_receive){ - past_lift_gray = RCData[2].lift_gray; - first_receive = false; + 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(RCData[2].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(RCData[2].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]; } } - 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 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]; - } + else{ + lift_nextTarg = lift_preset[lift_nearest_num]; } - - lift_stepTarg = lift_nextTarg; - lift_stepMoving = true; - } + lift_stepTarg = lift_nextTarg; + lift_stepMoving = true; + } - past_lift_gray = RCData[2].lift_gray; - if(RCData[2].lift_down != 0)lift_inc = -1; - else if(RCData[2].lift_up != 0)lift_inc = 1; - else lift_inc = 0; - - } // すっきりした // してない + } + past_lift_gray = buf->lift_gray; + + if(abs(getJoy7bit(buf->joyRY)) > 40 && abs(getJoy7bit(buf->joyRX)) < 10){ + if(getJoy7bit(buf->joyRY) > 0)lift_inc = 1; + else lift_inc = -1; + } + else{ + lift_inc = 0; + } }