yuto kawamura
/
F3RC_syudou_master
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
F3RC_syudou_master
by 
F3RC1班
Revision 19:a3f57c9833b6, committed 2017-08-23
- Comitter:
- yuto17320508
- Date:
- Wed Aug 23 10:10:26 2017 +0000
- Parent:
- 18:2579c275ef57
- Commit message:
- a
Changed in this revision
| User.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2579c275ef57 -r a3f57c9833b6 User.cpp
--- 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);