Diff: main.cpp

Revision:

9:95f6b9f54395

Parent:

8:9656790eea17

Child:

10:c1ca3db7177c

--- a/main.cpp	Tue Jul 20 06:41:29 2021 +0000
+++ b/main.cpp	Tue Jul 20 08:57:36 2021 +0000
@@ -21,39 +21,35 @@
 int state = 0;
 
 ////角度制御(アブソリュートエンコーダ)/////
-DigitalOut myled1(LED1);
 Timer timer;
 CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX);   //速度制御のPD計算
 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算
 CalPID angle_omega_pid(5.0,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
 Amt21 amt(p9,p10,p8);    //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
 MotorController motor(p26,p25,DELTA_T,amt,speed_pid,angle_duty_pid,angle_omega_pid);    //MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega)
-double radToDeg(double x);
 double degToRad(double x);
-double target_speed=0;
 double target_rad=0;
 double target_deg = 0;
 double turn_deg = 0;
 double turn_rad = 0;
+DigitalIn toggle1(p16, PullUp);     //トグルright(マイコン側)
+DigitalIn toggle2(p20, PullUp);     //トグルleft(外側)
 
 ////先端アーム(サーボ)/////
 DigitalIn ts_c1(p6, PullUp);
 DigitalIn ts_c2(p7, PullUp);
 KondoServo servo_c(p13, p14);
 double servoDeg(double servo_value);
-void touchGrab();
-int id_c1 = 0;
+void touchGrab();   //タッチセンサ
 float deg_c_grab = 10600;
 float deg_c_open = 8300;
-int hand = 0;   //タッチセンサがONになった状態:1，OFF:0
 
-////データ記録まわり/////
-int angle_count=0;
-int omega_count=0;
+////データ記録関連/////
 double now_deg=0;
 double now_count=0;
 
 /////////////////               ROS               /////////////////
+DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
 DigitalOut myled4(LED4);
@@ -66,9 +62,6 @@
 ros::NodeHandle  nh;
 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
 
-DigitalIn toggle1(p16, PullUp);     //right
-DigitalIn toggle2(p20, PullUp);     //left
-
 int main()
 {
     myled1 = 0;     //目標角度以上離れている:点灯
@@ -159,10 +152,6 @@
 {
     return (servo_value - 3500)*270/(11500 - 3500);
 }
-double radToDeg(double x)
-{
-    return x*180/M_PI;
-}
 double degToRad(double x)
 {
     return x*M_PI/180;