Akito
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of
omuni_speed_pid
by 
shinji sawai
Diff: main.cpp
- Revision:
- 5:12be2ac0f395
- Parent:
- 4:5591d3c8a761
- Child:
- 6:22ad12682a5d
--- 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;
+ }
}