hirromu takaki
kourobo 2019 tag number 5 program
Dependencies: mbed omuni_power ikarashiMDC PS3
Revision 1:6078ee91e5d7, committed 2019-03-05
- Comitter:
- THtakahiro702286
- Date:
- Tue Mar 05 13:00:32 2019 +0000
- Parent:
- 0:b2c626a3336d
- Commit message:
- kourobo program
Changed in this revision
diff -r b2c626a3336d -r 6078ee91e5d7 main.cpp
--- a/main.cpp Thu Feb 28 05:33:24 2019 +0000
+++ b/main.cpp Tue Mar 05 13:00:32 2019 +0000
@@ -5,7 +5,7 @@
#define PI 3.14159265
Serial serial(PC_6,PC_7);
DigitalOut serialcontrol(D2);
-DigitalOut stop(PB_1,PB_15);
+DigitalOut stop(PB_12,PB_2);
ikarashiMDC ikarashi[] {
ikarashiMDC(&serialcontrol,2,0,SM,&serial),
@@ -14,7 +14,7 @@
ikarashiMDC(&serialcontrol,2,3,SM,&serial)
};
-PS3 ps3(PC_12, PD_2);
+PS3 ps3(PC_10, PC_11);
Serial pc(USBTX,USBRX,115200);
int main()
@@ -27,106 +27,102 @@
do{
for(int i = 0; i < 4; i++) {
neutral[i] = ps3.getStick(i);
- pc.printf("%4d",neutral[i]);
}
count++;
- } while(count <= 50);
+ } while(count <= 1152000);
for(int i = 0; i < 4; i++) {
neutral[i] = ps3.getStick(i);
- pc.printf("%4d",neutral[i]);
}
while(1){
stop = 1;
/*ボタン0=上 1=下 2=左 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/
for(int i = 0; i < 12; i++) {
b[i] = ps3.getButton(i);
- pc.printf("%2d",b[i]);
}
/*ジョイスティック*/
for(int i = 0; i < 4; i++) {
stick[i] = ps3.getStick(i);
- pc.printf("%4d",stick[i] - neutral[i]);
}
/*トリガースイッチ*/
for(int i = 0; i < 2; i++) {
trigger[i] = ps3.getTrigger(i);
- pc.printf("%4d",trigger[i]);
}
X = stick[0] - neutral[0];
Y = neutral[1] - stick[1];
+ if(abs(X) <= 20){
+ X = 0;
+ }
+ if(abs(Y) <= 20) {
+ Y = 0;
+ }
/*微調整用コマンド*/
power = 1;
- if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
+ if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
X = 0;
- Y = 1;
- power = 0.2;
+ Y = 128;
}
- if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
+ if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
X = 0;
- Y = -1;
- power = 0.2;
+ Y = -128;
}
- if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
- X = -1;
+ if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && X == 0 && Y == 0) {
+ X = -128;
Y = 0;
- power = 0.2;
}
- if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
- X = 1;
+ if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && X == 0 && Y == 0) {
+ X = 128;
Y = 0;
- power = 0.2;
}
/*オムニ制御*/
- if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+ if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || (X == 0 && Y == 0)) {
acc[0] = 0;
acc[1] = 0;
acc[2] = 0;
} else {
- acc[0] = omuni_speed(X,Y,PI * 1 / 3) * power;
- acc[1] = omuni_speed(X,Y,PI * 3 / 3) * power;
- acc[2] = omuni_speed(X,Y,PI * 5 / 3) * power;
+ acc[0] = omuni_speed(X,Y,PI * 1 / 6) * power;
+ acc[1] = omuni_speed(X,Y,PI * 9 / 6) * power;
+ acc[2] = omuni_speed(X,Y,PI * 5 / 6) * power;
}
- if((trigger[0] <= 200 && trigger[1] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+ if((trigger[0] <= 200 && trigger[1] >= 200) && (X == 0 && Y == 0)) {
acc[0] = -0.5;
acc[1] = -0.5;
acc[2] = -0.5;
}
- if((trigger[1] <= 200 && trigger[0] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+ if((trigger[1] <= 200 && trigger[0] >= 200) && (X == 0 && Y == 0)) {
acc[0] = 0.5;
acc[1] = 0.5;
acc[2] = 0.5;
}
/*機構制御*/
if(b[4] == 1 && b[5] == 0){
- acc[3] = -0.2;
+ acc[3] = 0.4;
}
if(b[4] == 0 && b[5] == 1){
- acc[3] = 0.2;
+ acc[3] = -0.4;
}
if(b[4] == 0 && b[5] == 0){
acc[3] = 0;
}
/*速度制限*/
- for(int i = 0;i < 4;i++) {
- if(acc[i] >= 0.5){
- acc[i] = 0.5;
- }
- if(acc[i] <= -0.5) {
- acc[i] = -0.5;
+ if (b[7] == 1) {
+ for(int i = 0;i < 4;i++) {
+ if(acc[i] >= 0.5){
+ acc[i] = 0.5;
+ }
+ if(acc[i] <= -0.5) {
+ acc[i] = -0.5;
+ }
}
}
- /*タイムアウト制御 & 非常停止*/
+ /*タイムアウト制御*/
if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) {
- stop = 0;
+ acc[0] = 0;
+ acc[1] = 0;
+ acc[2] = 0;
}
-
ikarashi[0].setSpeed(acc[0]);
ikarashi[1].setSpeed(acc[1]);
ikarashi[2].setSpeed(acc[2]);
ikarashi[3].setSpeed(acc[3]);
- for(int i = 0;i < 5;i++) {
- pc.printf(" %f",acc[i]);
- }
- pc.printf("\r\n");
}
}
\ No newline at end of file
diff -r b2c626a3336d -r 6078ee91e5d7 mbed.bld --- a/mbed.bld Thu Feb 28 05:33:24 2019 +0000 +++ b/mbed.bld Tue Mar 05 13:00:32 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r b2c626a3336d -r 6078ee91e5d7 omuni_power.lib --- a/omuni_power.lib Thu Feb 28 05:33:24 2019 +0000 +++ b/omuni_power.lib Tue Mar 05 13:00:32 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/THtakahiro702286/code/omuni_power/#dfeb240dbbc1 +https://os.mbed.com/users/THtakahiro702286/code/omuni_power/#794a087f6216