日記
主導機 mbed用のプログラムです 改良しました
Dependencies: mbed
Fork of
F3RC_syudou_master
by 
日記
Revision 19:a3f57c9833b6, committed 2017-08-23
- Comitter:
- yuto17320508
- Date:
- Wed Aug 23 10:10:26 2017 +0000
- Parent:
- 18:2579c275ef57
- Child:
- 20:b84beed117ef
- Commit message:
- a
Changed in this revision
| User.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);