ロボステ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
main.cpp
- Committer:
- yuki0701
- Date:
- 2020-03-04
- Revision:
- 5:f9dd9346bfd4
- Parent:
- 4:f1f638a6b6c5
File content as of revision 5:f9dd9346bfd4:
#include "mbed.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
//#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);//sponge
DigitalOut pass1(p27);
DigitalOut pass2(p28);
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 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()
{
servo.set_degree(ID, 80); //サーボを動かす(ID,動かしたい角度)
}
void put()
{
servo.set_degree(ID, 100); //サーボを動かす(ID,動かしたい角度)
}
void top()
{
servo.set_degree(ID, 90); //サーボを動かす(ID,動かしたい角度)
}
int q = 0;
void can_read()
{
//CANMessage msg;
arm_num.x = T2;
arm_num.y = (float)kkk;
arm_num.z = 12;
pub.publish(&arm_num);
if(t2_r > T2) {
T2 = t2_r;
}
}
int main()
{
//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);
//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
can_ticker.attach(&can_read,0.01);
printf("wait_mode\r\n");
//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;
wait(1);
pass2=0;//動き出した瞬間に展開始めるやつ
while(1) {
printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
nh.spinOnce();
wait(0.01);
if(T2 == 2) {
break;
}
}
}
if(T2 == 2) { //ボール掴んで投げる
//printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
tsukami();
wait(3);
snatch=1;
wait(1);
put();
wait(3);
snatch=0;
wait(1);
top();
wait(5);
pass1=0;
wait(5);
//wait(5);
T2++;
}
if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
while(1) {
printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
nh.spinOnce();
wait(0.01);
if(T2 == 4) {
break;
}
}
}
printf("end\n\r");
#endif
#ifdef NHK2020_MAIN_MODE2
//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) {
nh.spinOnce();
wait(0.01);
printf("t2=1");
if(T2 == 1) {
break;
}
}
}
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) { //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) { //idou
//T2=4;
while(1) {
nh.spinOnce();
wait(0.01);
printf("t2=3");
if(T2 == 4) {
break;
}
}
}
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) { //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) { //ボール掴んで投げる
snatch=1;
tsukami();
printf("tsukami");
wait(3);
snatch=0;
wait(1);
put();
printf("put");
wait(3);
snatch=1;
wait(1);
top();
printf("top");
wait(5);
pass1=0;
wait(5);
pass1=1;
pass2=1;
wait(5);
T2++;
//T2=7;
//while(1) {
// printf("t2=6");
// if(T2 == 7) {
// break;
// }
// }
}
if(T2 == 7) { //移動
//T2=8;
pass2=0;
wait(1);
while(1) {
nh.spinOnce();
wait(0.01);
printf("t2=7");
if(T2 == 8) {
break;
}
}
}
if(T2 == 8) { //ボール掴んで投げる
snatch=1;
tsukami();
printf("tsukami");
wait(3);
snatch=0;
wait(1);
put();
printf("put");
wait(3);
snatch=1;
wait(1);
top();
printf("top");
wait(5);
pass1=0;
wait(5);
//wait(5);
pass1=1;
pass2=1;
wait(5);
T2++;
//T2=9;
//while(1) {
// printf("t2=8");
// if(T2 == 9) {
// break;
// }
// }
}
if(T2 == 9) { //idou
pass2=0;
wait(1);
//T2=10;
while(1) {
nh.spinOnce();
wait(0.01);
printf("t2=9");
if(T2 == 10) {
break;
}
}
}
if(T2 == 10) { //ボール掴んで投げる
snatch=1;
tsukami();
printf("tsukami");
wait(3);
snatch=0;
wait(1);
put();
printf("put");
wait(3);
snatch=1;
wait(1);
top();
printf("top");
wait(5);
pass1=0;
wait(5);
//wait(5);
T2++;
//T2=9;
//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;
}
}
}
#endif
#ifdef NHK2020_MAIN_MODE3
can1.frequency(1000000);
can_ticker.attach(&can_read,0.01);
//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) { //スタート位置からkick前まで移動（アーム待機）
//T2=2;
while(1) {
nh.spinOnce();
wait(0.01);
if(T2 == 2) {
break;
}
}
}
if(T2 == 2) { //idou
//T2=3;
while(1) {
nh.spinOnce();
wait(0.01);
if(T2 == 3) {
break;
}
}
}
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) { //kitaku
//T2=5;
while(1) {
nh.spinOnce();
wait(0.01);
if(T2 == 5) {
break;
}
}
}
#endif
}