DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 2:b728a6564520
- Parent:
- 1:c0a3e4589a8f
- Child:
- 3:78b608ba0286
--- a/main.cpp Thu Jul 08 11:48:58 2021 +0000 +++ b/main.cpp Thu Jul 08 12:55:18 2021 +0000 @@ -5,22 +5,9 @@ #include "KondoServo.h" #include "MotorController.h" #include <stdlib.h> - -///////////////// ROS ///////////////// #include <ros.h> #include <std_msgs/String.h> #include <geometry_msgs/Point.h> -DigitalOut myled2(LED2); -DigitalOut myled3(LED3); -DigitalOut myled4(LED4); -float arm_num=0,arm_rad=0; -void cArmCallback(const geometry_msgs::Point &arm_state) -{ - arm_num = arm_state.x; //右回転=0、左回転=1 - arm_rad = arm_state.y; //目標角度 -} -ros::NodeHandle nh; -ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); ///////////////// 宣言部分 ////////////////////// #ifndef M_PI @@ -43,6 +30,7 @@ double target_rad=0; double target_deg = 0; double radToDeg(double x); +double degToRad(double x); ////先端アーム(サーボ)///// DigitalIn ts_c1(p6, PullUp); @@ -55,13 +43,25 @@ float deg_c_open = 7500; int hand = 0; - ////データ記録まわり///// int angle_count=0; int omega_count=0; double now_deg=0; //度数法 double now_count=0; +///////////////// ROS ///////////////// +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); +float arm_num=0,arm_deg=0,arm_rad=0; +void cArmCallback(const geometry_msgs::Point &arm_state) +{ + arm_num = arm_state.x; //右回転=1、左回転=2 + arm_deg = arm_state.y; //目標角度 +} +ros::NodeHandle nh; +ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); + int main() { myled1 = 0; //アブソリュートエンコーダ用if文中で点灯 @@ -75,15 +75,17 @@ motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より -// target_rad = -(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) ////////////////////////// -// target_deg = radToDeg(target_rad); + target_rad = -(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) ////////////////////////// + target_deg = radToDeg(target_rad); while (1) { //// ROS //// nh.spinOnce(); - target_rad = arm_rad; - if(arm_num==1&&arm_rad==-(M_PI/6)) { //LEDは動作確認用 + if(arm_num==1&&arm_deg==30) { //LEDは動作確認用 myled2 = !myled2; + myled3 = !myled3; + arm_rad = degToRad(arm_deg); +// target_rad = arm_rad; } ////タッチセンサ//// @@ -98,11 +100,12 @@ ////アブソリュートエンコーダ//// if(timer.read()>DELTA_T) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す - myled1=!myled1; //LED2 点灯 + myled1=1; //LED2 点灯 motor.AcOmega(target_rad); //速度式角度制御 } else { //目標角度に達したら停止 myled1=0; //LED2 消灯 motor.stop(); + //wait(100); //table停止用時間 servo_c.set_degree(0,servoDeg(7500)); //servo_c離す } amt.rewriteCount(); @@ -127,4 +130,8 @@ double radToDeg(double x) { return x*180/M_PI; +} +double degToRad(double x) +{ + return x*M_PI/180; } \ No newline at end of file