ロボステ6期
/
NHK2020-arm-sub4-3-4
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.
Dependencies: mbed KondoServoLibrary ros_lib_kinetic
Diff: main.cpp
- Revision:
- 5:f9dd9346bfd4
- Parent:
- 4:f1f638a6b6c5
--- a/main.cpp Tue Dec 24 10:24:35 2019 +0000
+++ b/main.cpp Wed Mar 04 04:05:04 2020 +0000
@@ -1,72 +1,73 @@
#include "mbed.h"
-#include "EC.h" //Encoderライブラリをインクルード
-#include "SpeedController.h"
+#include "KondoServo.h"
+
+#include <ros.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/String.h>
+
+ros::NodeHandle nh;
+geometry_msgs::Point arm_num;
+
+ros::Publisher pub("/arm_rt",&arm_num);
+
+
#define RESOLUTION 2048//分解能
//#define NHK2020_TEST_MODE
//#define NHK2020_MAIN_MODE
#define NHK2020_MAIN_MODE2
-CAN can1(p30,p29);
+//#define NHK2020_MAIN_MODE3
+//CAN can1(p30,p29);
Ticker can_ticker; //can用ticker
-Ec4multi EC_backdrop(p15,p16,RESOLUTION);
-SpeedControl backdrop(p22,p21,50,EC_backdrop);
-Serial pc(USBTX,USBRX);
-DigitalOut snatch(p8);
+//Ec4multi EC_backdrop(p15,p16,RESOLUTION);
+//SpeedControl backdrop(p22,p21,50,EC_backdrop);
+///////////Serial pc(USBTX,USBRX);
+DigitalOut snatch(p8);//sponge
DigitalOut pass1(p27);
DigitalOut pass2(p28);
-DigitalOut pakapaka1(p14);
+DigitalOut tkeep(p11);
+DigitalOut kick(p12);
+KondoServo servo(p13, p14, 1, 115200); //ピン宣言 (TX,RX,不明,通信の周期？)
+
+
+
+int ID=0; //ICSマネージャーを使って変更可能（このプログラムそのまま使って動かなかったらIDが違う説濃厚）
//Ticker motor_tick; //角速度計算用ticker
char t2[1]= {0}; //動作番号（送信する値（char型））
int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
-double a=0;//now
-double b=0;//target
double turn=0;
int k = 0;
+int kkk = 0;
+
+double gb1, gb2, gb3;
+void messageCallback(const geometry_msgs::Quaternion &arm_get)
+{
+ t2_r = (int)arm_get.x;
+ gb1 = (double)arm_get.y;
+ gb2 = (double)arm_get.z;
+ gb3 = (double)arm_get.w;
+}
+
+ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_arm", &messageCallback);
+
void tsukami()
{
- b=1.3;
+ servo.set_degree(ID, 80); //サーボを動かす(ID,動かしたい角度)
}
void put()
{
- b=-0.7;
+ servo.set_degree(ID, 100); //サーボを動かす(ID,動かしたい角度)
}
void top()
{
- b=0;
+ servo.set_degree(ID, 90); //サーボを動かす(ID,動かしたい角度)
}
-void move_motor()
-{
- while(1) {
- double old_turn=turn;
- a=EC_backdrop.getRad();
- if(a-b>=0.1) {
- turn=0.1;
- pc.printf("F");
- } else if (a-b>=0.05) {
- turn=10*(a-b)*(a-b);
- pc.printf("f");
- } else if (b-a>=0.1) {
- turn=-0.1;
- pc.printf("B");
- } else if (b-a>=0.05) {
- turn=-10*(a-b)*(a-b);
- pc.printf("b");
- } else {
- backdrop.stop();
- backdrop.turn(0);
- turn=0;
- pc.printf("s");
- }
- if(turn*old_turn<0)turn=0;
- backdrop.turn(turn);
- pc.printf("%lf",EC_backdrop.getRad());
- if(b-a<0.05&&a-b<0.05)break;
- }
-}
+
int q = 0;
@@ -74,33 +75,13 @@
{
- CANMessage msg;
-
- if(can1.read(msg)) {
+ //CANMessage msg;
- if(msg.id == 1) {
- t2_r = msg.data[0];
- //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r);
- } else {
- if(q > 100) {
- //printf("id1 fale\n\r");
- q = 0;
- }
- q++;
- }
+ arm_num.x = T2;
+ arm_num.y = (float)kkk;
+ arm_num.z = 12;
- } else {
- if(k > 100) {
- //printf("arm fale\n\r");
- k = 0;
- }
- k++;
- }
-
- t2[0] = T2;
-
- if(can1.write(CANMessage(2,t2,1))) {
- }
+ pub.publish(&arm_num);
if(t2_r > T2) {
T2 = t2_r;
@@ -112,111 +93,96 @@
int main()
{
- pc.printf("setting please");
- move_motor();
+ //pc.printf("setting please");
-
-
-
+ nh.getHardware()->setBaud(115200);
+ nh.initNode();
+ nh.advertise(pub);
+ nh.subscribe(sub);
+// snatch=0,pass1=0,pass2=0,ttsukami=0,kick=0;
+// servo.init(); //servo初期化？ 決まり文句
#ifdef NHK2020_TEST_MODE
while(1) {
printf("T2 = %d\n\r",T2);
- pc.printf("%lf",EC_backdrop.getRad());
- if(pc.readable()) {
- char sel=pc.getc();
- if(sel=='z') {
- pc.printf("z\r\n");
- tsukami();
- } else if(sel=='x') {
- pc.printf("x\r\n");
- put();
- } else if(sel=='c') {
- pc.printf("c\r\n");
- top();
- }
- /* if(sel=='q') {
- printf("\r\n");
- if(denjiben==0)denjiben=1;
- else denjiben=0;
- }*/
- if(sel=='1') {
- snatch=0;
- printf("snatch_off\r\n");
- }
- if(sel=='2') {
- snatch=1;
- printf("snatch_on\r\n");
- }
- if(sel=='3') {
- pass1=0;
- printf("pass1_off\r\n");
- }
- if(sel=='4') {
- pass1=1;
- printf("pass1_on\r\n");
- }
- if(sel=='5') {
- pass2=0;
- printf("pass2_off\r\n");
- }
- if(sel=='6') {
- pass2=1;
- printf("pass2_on\r\n");
- }
- // if(sel=='a') {
- // //pc.printf("x\r\n");
- // //put();
- // }
- }
+ //printf("%lf",EC_backdrop.getRad());
+ /* if(sel=='z') {
+ pc.printf("z\r\n");
+ tsukami();
+ } else if(sel=='x') {
+ pc.printf("x\r\n");
+ put();
+ } else if(sel=='c') {
+ pc.printf("c\r\n");
+ top();
+ }
+ if(sel=='q') {
+ printf("\r\n");
+ if(denjiben==0)denjiben=1;
+ else denjiben=0;
+ }
+ if(sel=='1') {
+ snatch=0;
+ printf("snatch_off\r\n");
+ }
+ if(sel=='2') {
+ snatch=1;
+ printf("snatch_on\r\n");
+ }
+ if(sel=='3') {
+ pass1=0;
+ printf("pass1_off\r\n");
+ }
+ if(sel=='4') {
+ pass1=1;
+ printf("pass1_on\r\n");
+ }
+ if(sel=='5') {
+ pass2=0;
+ printf("pass2_off\r\n");
+ }
+ if(sel=='6') {
+ pass2=1;
+ printf("pass2_on\r\n");
+ }*/
+
+
}
#endif
#ifdef NHK2020_MAIN_MODE
- can1.frequency(1000000);
can_ticker.attach(&can_read,0.01);
printf("wait_mode\r\n");
- //printf("T2 = %d\n\r",T2);
- if(T2 == 0) {
- while(1) {
- char sel=pc.getc();
- if(sel=='1') {
- snatch=0;
- printf("snatch_off\r\n");
- }
- if(sel=='2') {
- snatch=1;
- printf("snatch_on\r\n");
- }
- if(sel=='3') {
- pass1=0;
- printf("pass1_off\r\n");
- }
- if(sel=='4') {
- pass1=1;
- printf("pass1_on\r\n");
- }
- if(sel=='5') {
- pass2=0;
- printf("pass2_off\r\n");
- }
- if(sel=='6') {
- pass2=1;
- printf("pass2_on\r\n");
- }
- if(sel=='q') {
- printf("end_wait_mode\r\n");
- T2=1;
- break;
+
+
+
+ //while(1){
+ // if()brake;
+ // }
+ //ボタン操作待ち(セミオートセット
+
- }
+ //snatch=1;
+ pass1=1;
+ wait(1);
+ pass2=1;
+ wait(5);
+ while(1) {
+ printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
+ nh.spinOnce();
+ wait(0.01);
+ if(T2 == 1) {
+ break;
}
}
+ //T2 = 1;
if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
//T2=1;
- pass2=0;
+ wait(1);
+ pass2=0;//動き出した瞬間に展開始めるやつ
while(1) {
printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
- wait(0.1);
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 2) {
break;
}
@@ -225,22 +191,15 @@
if(T2 == 2) { //ボール掴んで投げる
//printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
+ tsukami();
+ wait(3);
snatch=1;
- tsukami();
- printf("tsukami");
- move_motor();
+ wait(1);
+ put();
wait(3);
snatch=0;
wait(1);
- put();
- printf("put");
- move_motor();
- wait(3);
- snatch=1;
- wait(1);
top();
- printf("top");
- move_motor();
wait(5);
pass1=0;
wait(5);
@@ -251,7 +210,8 @@
if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
while(1) {
printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
- wait(0.1);
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 4) {
break;
}
@@ -261,68 +221,93 @@
#endif
#ifdef NHK2020_MAIN_MODE2
- can1.frequency(1000000);
+ //can1.frequency(1000000);
can_ticker.attach(&can_read,0.01);
//printf("T2 = %d\n\r",T2);
if(T2 == 0) {
//T2=1;
+ /* while(1) {
+ char sel=pc.getc();
+ if(sel=='1') {
+ snatch=0;
+ printf("snatch_off\r\n");
+ }
+ if(sel=='2') {
+ snatch=1;
+ printf("snatch_on\r\n");
+ }
+ if(sel=='3') {
+ pass1=0;
+ printf("pass1_off\r\n");
+ }
+ if(sel=='4') {
+ pass1=1;
+ printf("pass1_on\r\n");
+ }
+ if(sel=='5') {
+ pass2=0;
+ printf("pass2_off\r\n");
+ }
+ if(sel=='6') {
+ pass2=1;
+ printf("pass2_on\r\n");
+ }
+ if(sel=='q') {
+ printf("end_wait_mode\r\n");
+ T2=1;
+ break;
+
+ }*/
+
+ snatch=1;
+ nh.spinOnce();
+ wait(5);
+ //T2=1;
while(1) {
- char sel=pc.getc();
- if(sel=='1') {
- snatch=0;
- printf("snatch_off\r\n");
- }
- if(sel=='2') {
- snatch=1;
- printf("snatch_on\r\n");
- }
- if(sel=='3') {
- pass1=0;
- printf("pass1_off\r\n");
- }
- if(sel=='4') {
- pass1=1;
- printf("pass1_on\r\n");
- }
- if(sel=='5') {
- pass2=0;
- printf("pass2_off\r\n");
- }
- if(sel=='6') {
- pass2=1;
- printf("pass2_on\r\n");
- }
- if(sel=='q') {
- printf("end_wait_mode\r\n");
- T2=1;
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=1");
+ if(T2 == 1) {
break;
-
}
}
}
- if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
+ if(T2 == 1) { //idou
//T2=2;
pass2=0;
+ printf("t2=1");
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=1");
if(T2 == 2) {
break;
}
}
}
- if(T2 == 2) { //ボール掴んで投げる
- //pakapaka=1;
+ if(T2 == 2) { //idou
//T2=3;
while(1) {
+ kkk++;
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=2");
+ wait(5);
+ T2 = 3;
if(T2 == 3) {
break;
}
+
}
}
- if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
+ if(T2 == 3) { //idou
//T2=4;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=3");
if(T2 == 4) {
break;
}
@@ -330,59 +315,78 @@
}
if(T2 == 4) { //kikk
//T2=5;
+ wait(1);
+ tkeep=1;
+ wait(1);
+ snatch=0;
+ wait(1);
+ kick=1;
T2++;
+
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=4");
if(T2 == 5) {
break;
}
}
}
-
+
- if(T2 == 5) { //ボール掴んで投げる
+ if(T2 == 5) { //idou
+ pass1=1;
+ pass2=1;
+ wait(2);
+ pass2=0;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=5");
if(T2 == 6) {
break;
}
}
}
- if(T2 == 6) { //スタートゾーンに戻る（アーム待機）
+ if(T2 == 6) { //ボール掴んで投げる
snatch=1;
tsukami();
printf("tsukami");
- move_motor();
wait(3);
snatch=0;
wait(1);
put();
printf("put");
- move_motor();
wait(3);
snatch=1;
wait(1);
top();
printf("top");
- move_motor();
wait(5);
pass1=0;
wait(5);
pass1=1;
pass2=1;
- //wait(5);
+ wait(5);
T2++;
//T2=7;
- while(1) {
- if(T2 == 7) {
- break;
- }
- }
+ //while(1) {
+// printf("t2=6");
+// if(T2 == 7) {
+// break;
+// }
+// }
}
- if(T2 == 7) { //スタート位置からボール前まで移動（アーム待機）
+ if(T2 == 7) { //移動
//T2=8;
pass2=0;
+ wait(1);
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=7");
if(T2 == 8) {
break;
}
@@ -393,19 +397,16 @@
snatch=1;
tsukami();
printf("tsukami");
- move_motor();
wait(3);
snatch=0;
wait(1);
put();
printf("put");
- move_motor();
wait(3);
snatch=1;
wait(1);
top();
printf("top");
- move_motor();
wait(5);
pass1=0;
wait(5);
@@ -413,19 +414,25 @@
pass1=1;
pass2=1;
+ wait(5);
T2++;
//T2=9;
- while(1) {
- if(T2 == 9) {
- break;
- }
- }
+ //while(1) {
+// printf("t2=8");
+// if(T2 == 9) {
+// break;
+// }
+// }
}
- if(T2 == 9) { //スタートゾーンに戻る（アーム待機）
+ if(T2 == 9) { //idou
pass2=0;
+ wait(1);
//T2=10;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=9");
if(T2 == 10) {
break;
}
@@ -435,19 +442,16 @@
snatch=1;
tsukami();
printf("tsukami");
- move_motor();
wait(3);
snatch=0;
wait(1);
put();
printf("put");
- move_motor();
wait(3);
snatch=1;
wait(1);
top();
printf("top");
- move_motor();
wait(5);
pass1=0;
wait(5);
@@ -455,15 +459,19 @@
T2++;
//T2=9;
- while(1) {
- if(T2 == 11) {
- break;
- }
- }
+ //while(1) {
+// printf("t2=10");
+// if(T2 == 11) {
+// break;
+// }
+// }
}
if(T2 == 11) { //スタートゾーンに戻る（アーム待機）
//T2=10;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
+ printf("t2=11");
if(T2 == 12) {
break;
}
@@ -476,42 +484,57 @@
//printf("T2 = %d\n\r",T2);
if(T2 == 0) {
//T2=1;
+ snatch=1;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 1) {
break;
}
}
}
- if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
+ if(T2 == 1) { //スタート位置からkick前まで移動（アーム待機）
//T2=2;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 2) {
break;
}
}
}
- if(T2 == 2) { //ボール掴んで投げる
- pakapaka=1;
+ if(T2 == 2) { //idou
//T2=3;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 3) {
break;
}
}
}
- if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
+ if(T2 == 3) { //kick
//T2=4;
+ wait(1);
+ tkeep=1;
+ wait(1);
+ snatch=0;
+ wait(1);
+ kick=1;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 4) {
break;
}
}
}
- if(T2 == 4) { //スタート位置からボール前まで移動（アーム待機）
- pakapaka=0;
+ if(T2 == 4) { //kitaku
//T2=5;
while(1) {
+ nh.spinOnce();
+ wait(0.01);
if(T2 == 5) {
break;
}