![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
主導機 mbed用のプログラムです
Fork of F3RC_syudou_master by
Diff: User.cpp
- Revision:
- 19:a3f57c9833b6
- Parent:
- 18:2579c275ef57
--- a/User.cpp Wed Aug 23 03:31:34 2017 +0000 +++ b/User.cpp Wed Aug 23 10:10:26 2017 +0000 @@ -28,8 +28,7 @@ PwmOut motor_r_2(p26); double fai=1/3*Pi;//φ - - +//個体差 double power_f=1; double power_l=1; double power_r=1; @@ -37,7 +36,8 @@ double M1; double M2; double M3; - +//回転の比 +double roll_spd=0.5; void motor_act() { if(M1 >=0) { @@ -170,80 +170,98 @@ cs = 0; spi. write(send); cs = 1; - printf("%d\r\n",send); + // printf("%d\r\n",send); + - if(LSX>=150 && LSX<=255 && LSY>=150 && LSY<=255) { //第一象限 - sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); - } else if(LSX>=0 && LSX<=80 && LSY>=150 && LSY<=255) { //第二象限 - sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); - } else if(LSX>=0 && LSX<=80 && LSY>=0 && LSY<=80) { //第三象限 - sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); - } else if(LSX>=150 && LSX<=255 && LSY>=0 && LSY<=80) { //第四象限 - sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; +//オムニホイールのプログラム + if(LSX>=110 && LSX<=145 && LSY>=110 && LSY<=145) { + M1=0; + M2=0; + M3=0; + if(RSX>=180 && RSX<=255) { + M1=power_f*roll_spd; + M2=power_l*roll_spd; + M3=power_r*roll_spd; + } else if(RSX>=0 && RSX<=80) { + M1=-1.0*power_f*roll_spd; + M2=-1.0*power_l*roll_spd; + M3=-1.0*power_r*roll_spd; + } + motor_act(); - } else if(LSX==255) { - sita = 0; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); - - } else if(LSY==255) { - sita = 90; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); + sita=0; + } else { - } else if(LSX==0) { - sita = 180; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); + if(LSX>=129 && LSX<=255 && LSY>=129 && LSY<=255) { //第四象限 + sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + } else if(LSX>=0 && LSX<=127 && LSY>=129 && LSY<=255) { //第三象限 + sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + } else if(LSX>=0 && LSX<=127 && LSY>=0 && LSY<=127) { //第二象限 + sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + } else if(LSX>=129 && LSX<=255 && LSY>=0 && LSY<=127) { //第一象限 + sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); +//真っすぐだけのプログラム(いらない) + } + /*else if(LSX==255) { + sita = 0; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); - } else if(LSY==0) { - sita = 270; - sita_2=90-sita; - M1=sin((sita_2-fai)*Pi/180)*power_f; - M2=sin((sita_2-fai+240)*Pi/180)*power_l; - M3=sin((sita_2-fai+120)*Pi/180)*power_r; - motor_act(); - } else { - motor_f_1=0; - motor_f_2=0; - motor_l_1=0; - motor_l_2=0; - motor_r_1=0; - motor_r_2=0; + } else if(LSY==255) { + sita = -90; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + + } else if(LSX==0) { + sita = 180; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + + } else if(LSY==0) { + sita = 90; + sita_2=90-sita; + M1=sin((sita_2-fai)*Pi/180)*power_f; + M2=sin((sita_2-fai+240)*Pi/180)*power_l; + M3=sin((sita_2-fai+120)*Pi/180)*power_r; + motor_act(); + } */ } + + printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);