9.4.2章节完成程序后小车运行不正常

[43996173]

错误：上传代码，开始小车运行正常一分钟左右后运行不正常

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include <PidController.h>
#include <Kinematics.h>
// 引入micro-ros和wifi相关头文件
#include <WiFi.h>
#include <micro_ros_platformio.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>


// put function declarations here:
//int myFunction(int, int);
Esp32McpwmMotor motor; //创建一个名为motor的对象
Esp32PcntEncoder encoders[2]; //创建1个数组用于存储两个编码器
PidController pid_controller[2]; //创建PID控制器对象数组
Kinematics kinematics;


/*
int64_t last_ticks[2];  //记录上一次读取的计数器数值
int32_t delta_ticks[2]; //记录两次读取之间的计数器差值
int64_t last_update_time; //记录上一次更新时间
float current_speeds[2]; //记录两个电机的速度
*/
float target_linear_speed = 50.0; //目标线速度，毫米每秒
float targer_angle_speed = 0.1f;  //目标角速度，弧度每秒
float out_left_speed;
float out_right_speed;

//声明相关的结构体对象
rcl_allocator_t allocator;  //内存分配器，用于动态内存分配管理
rclc_support_t support;     //用于存储时钟，内存分配器和上下文，提供支持
rclc_executor_t executor;   //执行器，用于管理订阅和计时器回调的执行
rcl_node_t node;            //节点
rcl_subscription_t subscriber;    //订阅者
geometry_msgs__msg__Twist sub_msg; // 存储订阅到的速度消息

void twist_callback(const void *msg_in) {
  // 将接收到的消息指针转化为geometry_msgs_msg_Twist 类型
  const geometry_msgs__msg__Twist *twist_msg = 
    (const geometry_msgs__msg__Twist *)msg_in;
  //运动学逆解并设置速度
  kinematics.kinematic_inverse(twist_msg->linear.x * 1000,
  twist_msg->angular.z, out_left_speed, out_right_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
}



//单独创建一个任务运行micro-ros,相当于一个线程
void micro_ros_task(void *parameter) {
  // 1、设置传输协议并延时等待设置完成
  IPAddress agent_ip;
  agent_ip.fromString("192.168.1.10"); //替换为ie自己的主机ip地址
  set_microros_wifi_transports("yuxuan01", "tingbo0207", agent_ip, 8888);
  delay(2000);
  // 2、初始化内存分配器
  allocator = rcl_get_default_allocator();
  //初始化 support
  rclc_support_init(&support, 0, NULL, &allocator);
  // 4、初始化节点
  rclc_node_init_default(&node, "fishbot_motion_control", "", &support);
  // 5、初始化执行器
  unsigned int num_handles = 0+1;
  rclc_executor_init(&executor, &support.context, num_handles, &allocator);
  // 6、初始化订阅者并添加到执行其中
  rclc_subscription_init_best_effort(
    &subscriber, &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel");
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg,
                                &twist_callback, ON_NEW_DATA);
  // 循环执行器
  rclc_executor_spin(&executor);
  }
/*
void motorSpeedControl() {
  //计算时间差
  uint64_t dt =  millis() - last_update_time;
  //计算编码器差值
  delta_ticks[0] = encoders[0].getTicks() - last_ticks[0];
  delta_ticks[1] = encoders[1].getTicks() - last_ticks[1];
  //距离比时间获取速度单位mm/ms，乘1000转换为mm/s,
  current_speeds[0] = float(delta_ticks[0] * 0.1051566) / dt * 1000;
  current_speeds[1] = float(delta_ticks[1] * 0.1051566) / dt * 1000;
  //更新数据
  last_update_time = millis();           //更新上一次更新时间
  last_ticks[0] = encoders[0].getTicks();//更新第一个编码器的计数器数值
  last_ticks[1] = encoders[1].getTicks();
  // 根据当前速度，更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, pid_controller[0].update(current_speeds[0]));
  motor.updateMotorSpeed(1, pid_controller[1].update(current_speeds[1]));
  //打印数据
  Serial.printf("spped1=%f mm/s,spped2=%f mm/s\n", current_speeds[0], current_speeds[1]);
}
*/
void setup() {
  // put your setup code here, to run once:
  //int result = myFunction(2, 3);
  Serial.begin(115200);
  // 初始化编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);

  //初始化电机

  motor.attachMotor(0, 22, 23); //将电机的0连接到引脚22和23
  motor.attachMotor(1, 12, 13); //将电机的1链接到引脚12和13
  //初始化PID控制器参数
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);
  //初始化轮子间距和电机参数
  kinematics.set_wheel_distance(175);
  kinematics.set_motor_param(0, 0.1051566);
  kinematics.set_motor_param(1, 0.1051566);
  //运动学逆解并设置速度
  kinematics.kinematic_inverse(target_linear_speed,targer_angle_speed,out_left_speed,out_right_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
  /*
  //初始化目标速度，单位 mm/s,使用毫米防止浮点运算丢失精度
  pid_controller[0].update_target(100);
  pid_controller[1].update_target(100);
  */
 // 创建任务运行micro_ros_task
 xTaskCreate(micro_ros_task,      //任务函数
            "micro_ros",          // 任务名称
            10240,                // 任务堆栈大小
            NULL,               // 传递任务函数的参数
            1,                   // 任务优先级
            NULL                 //任务句柄
            );

}

void loop() {
  delay(10); //等待10毫秒
  // 调用速度控制函数
  //motorSpeedControl(); 
  kinematics.update_motor_speed(millis(), encoders[0].getTicks(),
                                  encoders[1].getTicks());
  motor.updateMotorSpeed(
    0, pid_controller[0].update(kinematics.get_motor_speed(0)));
  motor.updateMotorSpeed(
    1, pid_controller[1].update(kinematics.get_motor_speed(1))
  );
  Serial.printf("x=%f,y=%f,angle=%f\n", kinematics.get_odom().x, 
               kinematics.get_odom().y, kinematics.get_odom().angle);
  
  }
  /*uint64_t dt = millis() - last_update_time; //计算时间差
  // 计算编码器差值
  delta_ticks[0] = encoders[0].getTicks() - last_ticks[0];
  delta_ticks[1] = encoders[1].getTicks() - last_ticks[1];
  //距离比时间获取速度 单位mm/ms 相当于m/s
  current_speeds[0] = float(delta_ticks[0] * 0.1051566) / dt;
  current_speeds[1] = float(delta_ticks[1] * 0.1051566) / dt;
  //更新数据
  last_update_time = millis(); //更新上一次更新时间
  last_ticks[0] = encoders[0].getTicks(); //更新第一个编码器的计数器数值
  last_ticks[1] = encoders[1].getTicks(); //更新第二个编码器的计数器数值


//打印数值
Serial.printf("spped1=%fm/s,spped2=%fm/s\n",current_speeds[0],current_speeds[1]);*/
//}

/*// put function definitions here:
int myFunction(int x, int y) {
  return x + y;
}*/

[43996173]

串行监视器日志：

---- 已打开串行端口 /dev/ttyUSB0 ----
�y=0.208421,angle=5.334671

x=-0.222831,y=0.051180,angle=5.825897
x=-0.222360,y=0.050949,angle=-0.456088
x=-0.221937,y=0.050742,angle=5.828926
x=-0.221515,y=0.050536,angle=-0.453688
x=-0.221043,y=0.050307,angle=5.830697
x=-0.220570,y=0.050078,angle=-0.451288
x=-0.220197,y=0.049897,angle=5.833097
x=-0.219724,y=0.049670,angle=-0.448888
x=-0.219250,y=0.049442,angle=5.835498
x=-0.218827,y=0.049239,angle=-0.447116
x=-0.218403,y=0.049036,angle=5.836640
x=-0.217929,y=0.048810,angle=-0.445345
x=-0.217455,y=0.048584,angle=5.839041
x=-0.216980,y=0.048359,angle=-0.442944
x=-0.216556,y=0.048158,angle=5.840812
x=-0.216081,y=0.047934,angle=-0.441173
x=-0.116134,y=0.170615,angle=4.028755
x=-0.020728,y=0.044835,angle=8.502912
x=-0.164437,y=-0.018979,angle=0.417898
x=-0.194232,y=0.135035,angle=4.903483
x=-0.037658,y=0.129818,angle=9.391469
x=-0.077170,y=-0.021561,angle=1.315484
x=-0.216130,y=0.050210,angle=5.806440
x=-0.115703,y=0.170035,angle=-2.268345
x=-0.020585,y=0.046017,angle=2.225069
x=-0.162296,y=-0.019778,angle=6.717854
x=-0.195639,y=0.132863,angle=-1.355731
x=-0.039401,y=0.132154,angle=3.137054
x=-0.074020,y=-0.020146,angle=7.630468
x=-0.215163,y=0.046731,angle=-0.442489
x=-0.119223,y=0.169976,angle=4.050925
x=-0.019762,y=0.049554,angle=8.544339
x=-0.159007,y=-0.021310,angle=0.470754
x=-0.197917,y=0.129951,angle=4.964168
x=-0.041863,y=0.134982,angle=-3.109360
x=-0.070774,y=-0.018559,angle=1.384682
x=-0.214319,y=0.042993,angle=5.878096
x=-0.123054,y=0.169739,angle=-2.194861
x=-0.019160,y=0.053121,angle=2.298553
x=-0.155563,y=-0.022959,angle=6.791967
x=-0.200195,y=0.126713,angle=-1.280990
x=-0.044402,y=0.137766,angle=3.212424
x=-0.067455,y=-0.016708,angle=7.705838
x=-0.213319,y=0.039279,angle=-0.366490
x=-0.127013,y=0.169452,angle=4.126924
x=-0.018697,y=0.056930,angle=8.620337
x=-0.152062,y=-0.024357,angle=0.547381
x=-0.202437,y=0.123481,angle=5.040794
x=-0.047186,y=0.140539,angle=-3.032162
x=-0.064261,y=-0.014710,angle=1.461252
x=-0.212093,y=0.035682,angle=5.954666
x=-0.130791,y=0.169037,angle=-2.118291
x=-0.018202,y=0.060632,angle=2.375123
x=-0.148385,y=-0.025659,angle=6.868537
x=-0.204336,y=0.120160,angle=-1.204420
x=-0.049845,y=0.143098,angle=3.288994
x=-0.061014,y=-0.012687,angle=7.782408
x=-0.210579,y=0.032128,angle=-0.291120
x=-0.134379,y=0.168526,angle=4.202922
x=-0.017832,y=0.064553,angle=8.696337
x=-0.144640,y=-0.026625,angle=0.623380
x=-0.205993,y=0.116950,angle=5.116222
x=-0.052469,y=0.145658,angle=-2.956734
x=-0.057700,y=-0.010494,angle=1.537308
x=-0.208934,y=0.028519,angle=6.030722
x=-0.138000,y=0.167667,angle=-2.042235
x=-0.017584,y=0.068200,angle=2.451179
x=-0.140834,y=-0.027734,angle=6.944593
x=-0.207703,y=0.113413,angle=-1.128363
x=-0.055401,y=0.148024,angle=3.365050
x=-0.054701,y=-0.008160,angle=7.858464
x=-0.207307,y=0.025085,angle=-0.214493
x=-0.141706,y=0.166825,angle=4.278921
x=-0.017601,y=0.072000,angle=8.772335
x=-0.137120,y=-0.028543,angle=0.699378
x=-0.209299,y=0.109964,angle=5.192792
x=-0.058393,y=0.150440,angle=-2.879536
x=-0.051666,y=-0.005600,angle=1.613878
x=-0.205442,y=0.021731,angle=6.107292
x=-0.145359,y=0.165897,angle=-1.965665
x=-0.017687,y=0.075932,angle=2.527749
x=-0.133238,y=-0.029148,angle=7.021163
x=-0.210708,y=0.106470,angle=-1.051794
x=-0.061476,y=0.152740,angle=3.442248
x=-0.048733,y=-0.002925,angle=7.935662
x=-0.203449,y=0.018451,angle=-0.137295
x=-0.148974,y=0.164829,angle=4.356119
x=-0.017926,y=0.079858,angle=8.849533
x=-0.129335,y=-0.029603,angle=0.776576
x=-0.211879,y=0.102929,angle=5.269419
x=-0.064533,y=0.154728,angle=-2.803538
x=-0.045875,y=-0.000394,angle=1.690504
x=-0.201291,y=0.015085,angle=6.183918
x=-0.152421,y=0.163427,angle=-1.889038
x=-0.018237,y=0.083500,angle=2.604375
x=-0.125404,y=-0.030118,angle=7.097789
x=-0.213029,y=0.099172,angle=-0.975168
x=-0.067792,y=0.156618,angle=3.518246
x=-0.043267,y=0.002371,angle=8.011660
x=-0.199159,y=0.011939,angle=-0.061297
x=-0.155955,y=0.162029,angle=4.432117
x=-0.018834,y=0.087253,angle=8.925530
x=-0.121611,y=-0.030350,angle=0.852574
x=-0.214080,y=0.095520,angle=5.345987
x=-0.071129,y=0.158438,angle=-2.726969
x=-0.040659,y=0.005198,angle=1.767073
x=-0.196804,y=0.008743,angle=6.260487
x=-0.159424,y=0.160389,angle=-1.812470
x=-0.019520,y=0.090960,angle=2.680944
x=-0.117640,y=-0.030627,angle=7.174986
x=-0.214974,y=0.091520,angle=-0.897970
x=-0.074606,y=0.159894,angle=3.594872
x=-0.038346,y=0.007977,angle=8.088285
x=-0.194512,y=0.005583,angle=0.015329
x=-0.162925,y=0.158540,angle=4.508742
x=-0.020482,y=0.094480,angle=9.002156
x=-0.113955,y=-0.030646,angle=0.929199
x=-0.215788,y=0.087776,angle=5.422613
x=-0.078073,y=0.161453,angle=-2.650343
x=-0.036071,y=0.011021,angle=1.843070
x=-0.192084,y=0.002600,angle=6.337113
x=-0.166423,y=0.156662,angle=-1.735844
x=-0.021693,y=0.098083,angle=2.756998
x=-0.110240,y=-0.030516,angle=7.250984
x=-0.216567,y=0.083888,angle=-0.821973
x=-0.081800,y=0.162937,angle=3.672069
x=-0.033931,y=0.014269,angle=8.165483
x=-0.189448,y=-0.000162,angle=0.092527
x=-0.169751,y=0.154776,angle=4.585940
x=-0.022791,y=0.101892,angle=9.079354
x=-0.106336,y=-0.030070,angle=1.006397
x=-0.217107,y=0.080115,angle=5.500440
x=-0.085537,y=0.164276,angle=-2.572517
x=-0.031967,y=0.017565,angle=1.920897
x=-0.186811,y=-0.002856,angle=6.414311
x=-0.173108,y=0.152727,angle=-1.658646
x=-0.024217,y=0.105554,angle=2.834768
x=-0.102607,y=-0.029534,angle=7.328182
x=-0.217440,y=0.076329,angle=-0.744775
x=-0.089160,y=0.165424,angle=3.748639
x=-0.030059,y=0.020853,angle=8.242052
x=-0.184016,y=-0.005432,angle=0.169095
x=-0.176321,y=0.150513,angle=4.663081
x=-0.025723,y=0.109112,angle=9.156495
x=-0.098850,y=-0.028896,angle=1.083538
x=-0.217677,y=0.072464,angle=5.576952
x=-0.092925,y=0.166436,angle=-2.496005
x=-0.028297,y=0.024249,angle=1.997409
x=-0.181127,y=-0.007948,angle=6.490823
x=-0.179454,y=0.148283,angle=-1.581505
x=-0.027371,y=0.112724,angle=2.911909
x=-0.095117,y=-0.028003,angle=7.405323
x=-0.217768,y=0.068696,angle=-0.667634
x=-0.096735,y=0.167411,angle=3.825779
x=-0.026668,y=0.027824,angle=8.319194
x=-0.178142,y=-0.010247,angle=0.246237
x=-0.182309,y=0.145833,angle=4.739079
x=-0.029002,y=0.115986,angle=9.232493
x=-0.091371,y=-0.027266,angle=1.160165
x=-0.217609,y=0.064700,angle=5.653579
x=-0.100417,y=0.167946,angle=-2.419378
x=-0.025094,y=0.031124,angle=2.074036
x=-0.175011,y=-0.012679,angle=6.567450
x=-0.185201,y=0.143174,angle=-1.505507
x=-0.030857,y=0.119265,angle=2.987907
x=-0.087622,y=-0.026186,angle=7.481892
x=-0.217310,y=0.060847,angle=-0.591065
x=-0.104183,y=0.168532,angle=3.902349
x=-0.023644,y=0.034714,angle=8.395763
x=-0.171762,y=-0.014832,angle=0.322807
x=-0.188053,y=0.140556,angle=4.816849
x=-0.032930,y=0.122804,angle=9.310834
x=-0.083972,y=-0.024805,angle=1.237877
x=-0.216969,y=0.057083,angle=5.731291
x=-0.108178,y=0.169223,angle=-2.341037
x=-0.022480,y=0.038708,angle=2.151805
x=-0.168558,y=-0.016720,angle=6.645848
x=-0.190922,y=0.137855,angle=-1.427109
x=-0.035236,y=0.126022,angle=3.065733
x=-0.080621,y=-0.023423,angle=7.559147
x=-0.216639,y=0.053341,angle=-0.513810
x=-0.112160,y=0.169436,angle=3.979604
x=-0.021534,y=0.042233,angle=8.473018
x=-0.165398,y=-0.018709,angle=0.400689
x=-0.193527,y=0.134871,angle=4.893532
x=-0.037453,y=0.128964,angle=9.386946
x=-0.077124,y=-0.022099,angle=1.313989
x=-0.216056,y=0.049377,angle=5.808031
x=-0.116141,y=0.169422,angle=-2.264926
x=-0.020667,y=0.045817,angle=2.228488
x=-0.161978,y=-0.020587,angle=6.722474
x=-0.196011,y=0.131794,angle=-1.351055
x=-0.039772,y=0.132012,angle=3.142988
x=-0.073487,y=-0.020491,angle=7.636402
x=-0.215024,y=0.045547,angle=-0.436555
x=-0.119817,y=0.169359,angle=4.056859
x=-0.019643,y=0.049530,angle=8.550273
x=-0.158372,y=-0.022221,angle=0.477316
x=-0.198273,y=0.128781,angle=4.970730
x=-0.042209,y=0.134928,angle=-3.102227
x=-0.070111,y=-0.018745,angle=1.391187
x=-0.214053,y=0.041873,angle=5.884601
x=-0.123716,y=0.169220,angle=-2.187784
x=-0.019000,y=0.053340,angle=2.305630
x=-0.154860,y=-0.023703,angle=6.799044
x=-0.200551,y=0.125649,angle=-1.273913
x=-0.044840,y=0.137805,angle=3.219501
x=-0.066710,y=-0.016897,angle=7.713543
x=-0.212915,y=0.038037,angle=-0.359413
x=-0.127532,y=0.168818,angle=4.134000
x=-0.018314,y=0.057094,angle=8.628043
x=-0.151048,y=-0.025218,angle=0.555086
x=-0.202528,y=0.122133,angle=5.048500
x=-0.047413,y=0.140387,angle=-3.024457
x=-0.063199,y=-0.015054,angle=1.469586
x=-0.211446,y=0.034104,angle=5.963000
x=-0.131259,y=0.168133,angle=-2.109957
x=-0.017850,y=0.060745,angle=2.383457
x=-0.147309,y=-0.026628,angle=6.876871
x=-0.204372,y=0.118706,angle=-1.196657
x=-0.050087,y=0.143029,angle=3.297957
x=-0.059860,y=-0.012850,angle=7.791370
x=-0.209894,y=0.030551,angle=-0.281586
x=-0.134883,y=0.167487,angle=4.211256
x=-0.017474,y=0.064488,angle=8.704670
x=-0.143518,y=-0.027743,angle=0.631713
x=-0.206279,y=0.115337,angle=5.125755
x=-0.053035,y=0.145507,angle=-2.947201
x=-0.056874,y=-0.010630,angle=1.546213
x=-0.208450,y=0.027035,angle=6.039627
x=-0.138860,y=0.166804,angle=-2.032758
x=-0.017506,y=0.068482,angle=2.460655
x=-0.139842,y=-0.028615,angle=6.954070
x=-0.208158,y=0.111898,angle=-1.118259
x=-0.056213,y=0.148046,angle=3.375155
x=-0.053935,y=-0.008122,angle=7.868569
x=-0.206869,y=0.023578,angle=-0.204388
x=-0.142704,y=0.165974,angle=4.289026
x=-0.017647,y=0.072408,angle=8.782440
x=-0.136144,y=-0.029337,angle=0.709483
x=-0.209774,y=0.108347,angle=5.203469
x=-0.059336,y=0.150323,angle=-2.869488
x=-0.051041,y=-0.005642,angle=1.623926
x=-0.205083,y=0.020142,angle=6.117340
x=-0.146453,y=0.164905,angle=-1.955617
x=-0.017883,y=0.076227,angle=2.537797
x=-0.132372,y=-0.030009,angle=7.031211
x=-0.211201,y=0.104824,angle=-1.041746
x=-0.062442,y=0.152591,angle=3.452296
x=-0.048135,y=-0.002937,angle=7.945710
x=-0.203058,y=0.016883,angle=-0.127247
x=-0.150057,y=0.163800,angle=4.366167
x=-0.018161,y=0.080150,angle=8.859581
x=-0.128465,y=-0.030425,angle=0.786624
x=-0.212438,y=0.101266,angle=5.280038
x=-0.065729,y=0.154693,angle=-2.792347
x=-0.045440,y=-0.000168,angle=1.701067
x=-0.201011,y=0.013668,angle=6.194481
x=-0.153711,y=0.162518,angle=-1.878476
x=-0.018690,y=0.084013,angle=2.614938
x=-0.124651,y=-0.030730,angle=7.108352
x=-0.213636,y=0.097626,angle=-0.964605
x=-0.069014,y=0.156604,angle=3.528809
x=-0.042756,y=0.002586,angle=8.022851
x=-0.198699,y=0.010317,angle=-0.049534
x=-0.157264,y=0.160905,angle=4.443880
x=-0.019272,y=0.087748,angle=8.937294
x=-0.120659,y=-0.031057,angle=0.864337
x=-0.214501,y=0.093730,angle=5.357180
x=-0.072263,y=0.158245,angle=-2.715777
x=-0.040080,y=0.005355,angle=1.778265
x=-0.196255,y=0.007152,angle=6.271679
x=-0.160575,y=0.159207,angle=-1.801278
x=-0.019902,y=0.091348,angle=2.692136
x=-0.116698,y=-0.031225,angle=7.185550
x=-0.215317,y=0.089887,angle=-0.887407
x=-0.075675,y=0.159842,angle=3.606007
x=-0.037542,y=0.008379,angle=8.100621
x=-0.193667,y=0.004058,angle=0.027664
x=-0.163891,y=0.157328,angle=4.520507
x=-0.020704,y=0.094948,angle=9.013921
x=-0.112698,y=-0.031269,angle=0.940964
x=-0.216027,y=0.085923,angle=5.435007
x=-0.079236,y=0.161301,angle=-2.637950
x=-0.035373,y=0.011402,angle=1.855464
x=-0.191177,y=0.001241,angle=6.348306
x=-0.167338,y=0.155545,angle=-1.724079
x=-0.021850,y=0.098738,angle=2.769335
x=-0.108855,y=-0.031036,angle=7.263377
x=-0.216592,y=0.082042,angle=-0.809580
x=-0.082807,y=0.162540,angle=3.683263
x=-0.033277,y=0.014417,angle=8.176677
x=-0.188668,y=-0.001857,angle=0.104348
x=-0.170804,y=0.153303,angle=4.597762
x=-0.023229,y=0.102160,angle=9.091176
x=-0.105208,y=-0.030780,angle=1.018219
x=-0.217166,y=0.078119,angle=5.511633
x=-0.086546,y=0.163748,angle=-2.561324
x=-0.031337,y=0.017646,angle=1.932090
x=-0.185943,y=-0.004507,angle=6.425504
x=-0.174076,y=0.151281,angle=-1.646824
x=-0.024660,y=0.105972,angle=2.847161
x=-0.101423,y=-0.029990,angle=7.340003
x=-0.217500,y=0.074508,angle=-0.732953
x=-0.090294,y=0.165225,angle=3.761089
x=-0.029398,y=0.021401,angle=8.254503
x=-0.183016,y=-0.006798,angle=0.181546
x=-0.177262,y=0.149231,angle=4.675532
x=-0.026161,y=0.109708,angle=9.168946
x=-0.097563,y=-0.029200,angle=1.095989
x=-0.217749,y=0.070632,angle=5.590032
x=-0.094236,y=0.166227,angle=-2.482925
x=-0.027856,y=0.024906,angle=2.009917
x=-0.180272,y=-0.009201,angle=6.503332
x=-0.180455,y=0.146984,angle=-1.569625
x=-0.027885,y=0.113319,angle=2.924417
x=-0.093925,y=-0.028162,angle=7.417260
x=-0.217839,y=0.066920,angle=-0.654497
x=-0.098095,y=0.167117,angle=3.838346
x=-0.026280,y=0.028422,angle=8.331759
x=-0.177263,y=-0.011550,angle=0.258803
x=-0.183583,y=0.144562,angle=4.752845
x=-0.029913,y=0.116925,angle=9.246830
x=-0.090354,y=-0.027037,angle=1.173302
x=-0.217790,y=0.063263,angle=5.666716
x=-0.101964,y=0.168039,angle=-2.406241
x=-0.024738,y=0.032219,angle=2.087801
x=-0.174037,y=-0.013643,angle=6.581215
x=-0.186372,y=0.142055,angle=-1.491742
x=-0.031713,y=0.120272,angle=3.001672
x=-0.086470,y=-0.025946,angle=7.495657
x=-0.217344,y=0.059294,angle=-0.577299
x=-0.105710,y=0.168526,angle=3.916114
x=-0.023337,y=0.035829,angle=8.409529
x=-0.170759,y=-0.015751,angle=0.336572
x=-0.189083,y=0.139355,angle=4.829986
x=-0.033741,y=0.123642,angle=9.323971
x=-0.082839,y=-0.024625,angle=1.251014
x=-0.216997,y=0.055453,angle=5.745056
x=-0.109727,y=0.168973,angle=-2.327900
x=-0.022220,y=0.039603,angle=2.165513
x=-0.167509,y=-0.017711,angle=6.658927
x=-0.191893,y=0.136559,angle=-1.414029
x=-0.036011,y=0.126849,angle=3.079384
x=-0.079272,y=-0.023282,angle=7.573427
x=-0.216372,y=0.051533,angle=-0.499530
x=-0.113527,y=0.169011,angle=3.993312
x=-0.021123,y=0.043155,angle=8.487298
x=-0.164103,y=-0.019835,angle=0.414970
x=-0.194519,y=0.133360,angle=4.908383
x=-0.038375,y=0.129771,angle=9.401797
x=-0.075797,y=-0.021865,angle=1.328840
x=-0.215678,y=0.047502,angle=5.822826
x=-0.117550,y=0.169012,angle=-2.250131
x=-0.020258,y=0.046832,angle=2.243283
x=-0.160654,y=-0.021596,angle=6.736697
x=-0.196951,y=0.130313,angle=-1.336260
x=-0.040785,y=0.132743,angle=3.157154
x=-0.072336,y=-0.020222,angle=7.650568
x=-0.214794,y=0.043805,angle=-0.422389
x=-0.121351,y=0.168953,angle=4.071025
x=-0.019490,y=0.050555,angle=8.564439
x=-0.157190,y=-0.023267,angle=0.492110
x=-0.199321,y=0.127129,angle=4.985524
x=-0.043420,y=0.135670,angle=-3.086861
x=-0.068957,y=-0.018413,angle=1.406553
x=-0.213813,y=0.039985,angle=5.899967
x=-0.125342,y=0.168697,angle=-2.172990
x=-0.018923,y=0.054378,angle=2.320424
x=-0.153541,y=-0.024718,angle=6.814410
x=-0.201521,y=0.123915,angle=-1.258547
x=-0.046014,y=0.138462,angle=3.234867
x=-0.065595,y=-0.016491,angle=7.728281
x=-0.212679,y=0.036209,angle=-0.344048
x=-0.129316,y=0.168286,angle=4.149366
x=-0.018502,y=0.058223,angle=8.642780
x=-0.150009,y=-0.026037,angle=0.569823
x=-0.203689,y=0.120634,angle=5.063237
x=-0.048860,y=0.141171,angle=-3.009719
x=-0.062531,y=-0.014365,angle=1.483123
x=-0.211431,y=0.032782,angle=5.976537
x=-0.133064,y=0.167884,angle=-2.096420
x=-0.018082,y=0.062176,angle=2.398194
x=-0.146239,y=-0.027096,angle=6.891608
x=-0.205539,y=0.117394,angle=-1.181349
x=-0.051652,y=0.143794,angle=3.311494
x=-0.059314,y=-0.012203,angle=7.804907
x=-0.209921,y=0.029163,angle=-0.268050
x=-0.136990,y=0.167280,angle=4.226564
x=-0.018018,y=0.066091,angle=8.719978
x=-0.142635,y=-0.028059,angle=0.647021
x=-0.207467,y=0.114034,angle=5.140435
x=-0.054713,y=0.146353,angle=-2.933093
x=-0.056252,y=-0.009879,angle=1.560950
x=-0.208365,y=0.025548,angle=6.054364
x=-0.140740,y=0.166334,angle=-2.018593
x=-0.018007,y=0.069741,angle=2.474821
x=-0.138954,y=-0.029079,angle=6.968235
x=-0.209141,y=0.110447,angle=-1.104722
x=-0.057700,y=0.148649,angle=3.388692
x=-0.053221,y=-0.007422,angle=7.882677
x=-0.206659,y=0.022032,angle=-0.189651
x=-0.144537,y=0.165276,angle=4.303191
x=-0.018168,y=0.073491,angle=8.796605
x=-0.135188,y=-0.030032,angle=0.724277
x=-0.210793,y=0.106634,angle=5.217690
x=-0.060966,y=0.150745,angle=-2.855266
x=-0.050369,y=-0.005030,angle=1.638719
x=-0.204776,y=0.018473,angle=6.132133
x=-0.148293,y=0.164087,angle=-1.940823
x=-0.018326,y=0.077372,angle=2.553219
x=-0.131163,y=-0.030617,angle=7.046633
x=-0.212061,y=0.102984,angle=-1.026324
x=-0.064096,y=0.152828,angle=3.466518
x=-0.047579,y=-0.002481,angle=7.959932
x=-0.202768,y=0.015134,angle=-0.113025
x=-0.151861,y=0.162791,angle=4.380389
x=-0.018691,y=0.081080,angle=8.874432
x=-0.127341,y=-0.031121,angle=0.801475
x=-0.213260,y=0.099308,angle=5.294888
x=-0.067297,y=0.154741,angle=-2.778640
x=-0.044783,y=0.000132,angle=1.715403
x=-0.200493,y=0.011644,angle=6.209388
x=-0.155416,y=0.161183,angle=-1.863569
x=-0.019328,y=0.084646,angle=2.629274
x=-0.123633,y=-0.031605,angle=7.122688
x=-0.214561,y=0.095451,angle=-0.949641
43773
x=-0.042384,y=0.003014,angle=8.037187
x=-0.198422,y=0.008509,angle=-0.035199
x=-0.159150,y=0.159676,angle=4.458215
x=-0.020124,y=0.088504,angle=8.951630
x=-0.119797,y=-0.031742,angle=0.878673
x=-0.215519,y=0.091672,angle=5.3
---- 已关闭串行端口 /dev/ttyUSB0，因为与计算机断开连接 ----

[43996173]

@43996173 串行日志前半部分

---- 已打开串行端口 /dev/ttyUSB0 ----
�y=0.208421,angle=5.334671
x=-0.408491,y=0.208040,angle=-0.947943
x=-0.408184,y=0.207614,angle=5.336442
x=-0.407877,y=0.207188,angle=-0.945543
x=-0.407634,y=0.206852,angle=5.338843
x=-0.407325,y=0.206427,angle=-0.943142
x=-0.407049,y=0.206047,angle=5.340614
x=-0.406740,y=0.205622,angle=-0.941371
x=-0.406430,y=0.205198,angle=5.343015
x=-0.406153,y=0.204819,angle=-0.939599
x=-0.405908,y=0.204484,angle=5.344786
x=-0.405597,y=0.204061,angle=-0.937199
x=-0.405289,y=0.203642,angle=5.345986
x=-0.405011,y=0.203263,angle=-0.936628
x=-0.404699,y=0.202841,angle=5.347758
x=-0.404387,y=0.202419,angle=-0.934227
x=-0.404107,y=0.202041,angle=5.350787
x=-0.403827,y=0.201664,angle=-0.931827
x=-0.403483,y=0.201202,angle=5.351986
x=-0.403169,y=0.200782,angle=-0.929999
x=-0.402888,y=0.200405,angle=5.355015
x=-0.402606,y=0.200029,angle=-0.927599
x=-0.402261,y=0.199569,angle=5.356215
x=-0.401945,y=0.199150,angle=-0.925770
x=-0.401629,y=0.198731,angle=5.358615
x=-0.401345,y=0.198356,angle=-0.922741
x=-0.401061,y=0.197981,angle=5.361015
x=-0.400777,y=0.197607,angle=-0.921599
x=-0.400459,y=0.197189,angle=5.362787
x=-0.400174,y=0.196815,angle=-0.919827
x=-0.399856,y=0.196398,angle=5.364558
x=-0.399537,y=0.195981,angle=-0.917427
x=-0.399251,y=0.195608,angle=5.366330
x=-0.398931,y=0.195192,angle=-0.915655
x=-0.398611,y=0.194776,angle=5.368730
x=-0.398324,y=0.194404,angle=-0.913884
x=-0.398036,y=0.194032,angle=5.371130
x=-0.397748,y=0.193661,angle=-0.911484
x=-0.397426,y=0.193246,angle=5.372901
x=-0.397103,y=0.192832,angle=-0.909084
x=-0.396814,y=0.192461,angle=5.374673
x=-0.396525,y=0.192091,angle=-0.907941
x=-0.396235,y=0.191721,angle=5.377073
x=-0.395880,y=0.191268,angle=-0.905484
x=-0.395556,y=0.190856,angle=5.378901
x=-0.395265,y=0.190487,angle=-0.903713
x=-0.394939,y=0.190075,angle=5.380673
x=-0.394648,y=0.189706,angle=-0.901941
x=-0.394322,y=0.189294,angle=5.382444
x=-0.394029,y=0.188926,angle=-0.898912
x=-0.393702,y=0.188516,angle=5.385473
x=-0.393343,y=0.188067,angle=-0.897084
x=-0.393050,y=0.187699,angle=5.386673
x=-0.392756,y=0.187332,angle=-0.895941
x=-0.392462,y=0.186966,angle=5.389073
x=-0.392133,y=0.186557,angle=-0.892912
x=-0.391772,y=0.186109,angle=5.390902
x=-0.391477,y=0.185744,angle=-0.891712
x=-0.391181,y=0.185378,angle=5.392044
x=-0.390851,y=0.184970,angle=-0.889941
x=-0.390554,y=0.184606,angle=5.395073
x=-0.390223,y=0.184199,angle=-0.886912
x=-0.389925,y=0.183834,angle=5.396844
x=-0.389628,y=0.183470,angle=-0.885770
x=-0.389295,y=0.183064,angle=5.398616
x=-0.388962,y=0.182659,angle=-0.883369
x=-0.388629,y=0.182253,angle=5.401016
x=-0.388294,y=0.181848,angle=-0.880969
x=-0.387995,y=0.181486,angle=5.402788
x=-0.387696,y=0.181124,angle=-0.879826
x=-0.387361,y=0.180719,angle=5.404559
x=-0.387060,y=0.180358,angle=-0.878055
x=-0.386760,y=0.179997,angle=5.406959
x=-0.386459,y=0.179636,angle=-0.875655
x=-0.386122,y=0.179233,angle=5.408731
x=-0.385785,y=0.178831,angle=-0.873255
x=-0.385447,y=0.178429,angle=5.411131
x=-0.385144,y=0.178069,angle=-0.870226
x=-0.384809,y=0.177672,angle=5.412960
x=-0.384506,y=0.177313,angle=-0.869654
x=-0.384166,y=0.176912,angle=5.414731
x=-0.383827,y=0.176512,angle=-0.867254
x=-0.383522,y=0.176153,angle=5.416502
x=-0.383253,y=0.175837,angle=-0.865483
x=-0.382913,y=0.175438,angle=5.418903
x=-0.382571,y=0.175039,angle=-0.863082
x=-0.382197,y=0.174602,angle=5.420732
x=-0.381927,y=0.174288,angle=-0.861254
x=-0.381620,y=0.173931,angle=5.422503
x=-0.381245,y=0.173495,angle=-0.860054
x=-0.380902,y=0.173098,angle=5.424332
x=-0.380559,y=0.172701,angle=-0.857653
x=-0.380251,y=0.172346,angle=5.427361
x=-0.379906,y=0.171950,angle=-0.854625
x=-0.379597,y=0.171596,angle=5.429132
x=-0.379252,y=0.171200,angle=-0.852853
x=-0.378943,y=0.170846,angle=5.430903
x=-0.378597,y=0.170452,angle=-0.851082
x=-0.378250,y=0.170057,angle=5.433304
x=-0.377940,y=0.169704,angle=-0.849310
x=-0.377629,y=0.169352,angle=5.434446
x=-0.377281,y=0.168958,angle=-0.847539
x=-0.376934,y=0.168565,angle=5.436847
x=-0.376622,y=0.168213,angle=-0.845767
x=-0.376273,y=0.167821,angle=5.438618
x=-0.375997,y=0.167511,angle=-0.843367
x=-0.375648,y=0.167119,angle=5.441019
x=-0.375298,y=0.166728,angle=-0.840967
x=-0.374984,y=0.166378,angle=5.442790
x=-0.374707,y=0.166069,angle=-0.839195
x=-0.374356,y=0.165679,angle=5.445190
x=-0.374041,y=0.165329,angle=-0.837424
x=-0.373693,y=0.164943,angle=5.445762
x=-0.373341,y=0.164553,angle=-0.836223
x=-0.373063,y=0.164246,angle=5.448162
x=-0.372710,y=0.163857,angle=-0.833823
x=-0.372394,y=0.163509,angle=5.449934
x=-0.372007,y=0.163084,angle=-0.832623
x=-0.371653,y=0.162696,angle=5.451762
x=-0.371299,y=0.162308,angle=-0.830223
x=-0.370981,y=0.161962,angle=5.454791
x=-0.370626,y=0.161576,angle=-0.827194
x=-0.370270,y=0.161190,angle=5.457191
x=-0.369880,y=0.160767,angle=-0.825365
x=-0.369523,y=0.160382,angle=5.459020
x=-0.369241,y=0.160078,angle=-0.822965
x=-0.368883,y=0.159693,angle=5.461421
x=-0.368563,y=0.159349,angle=-0.821194
x=-0.368205,y=0.158965,angle=5.463192
x=-0.367884,y=0.158622,angle=-0.819422
x=-0.367563,y=0.158279,angle=5.464334
x=-0.367204,y=0.157896,angle=-0.817651
x=-0.366882,y=0.157553,angle=5.467363
x=-0.366522,y=0.157171,angle=-0.814622
x=-0.366199,y=0.156830,angle=5.469135
x=-0.365876,y=0.156488,angle=-0.813479
x=-0.365481,y=0.156070,angle=5.470335
x=-0.365157,y=0.155730,angle=-0.811022
x=-0.364868,y=0.155425,angle=5.472163
x=-0.364544,y=0.155085,angle=-0.810451
x=-0.364220,y=0.154744,angle=5.473306
x=-0.363857,y=0.154365,angle=-0.808679
x=-0.363495,y=0.153985,angle=5.475706
x=-0.363169,y=0.153646,angle=-0.806908
x=-0.362844,y=0.153307,angle=5.476849
x=-0.362445,y=0.152892,angle=-0.804565
x=-0.362081,y=0.152514,angle=5.479820
x=-0.361681,y=0.152101,angle=-0.802737
x=-0.361354,y=0.151763,angle=5.482277
x=-0.360988,y=0.151387,angle=-0.799708
x=-0.360699,y=0.151090,angle=5.484678
x=-0.360297,y=0.150678,angle=-0.797879
x=-0.359930,y=0.150303,angle=5.486506
x=-0.359563,y=0.149928,angle=-0.795479
x=-0.359233,y=0.149593,angle=5.489535
x=-0.358865,y=0.149219,angle=-0.792450
x=-0.358534,y=0.148884,angle=5.491307
x=-0.358169,y=0.148514,angle=-0.791879
x=-0.357800,y=0.148141,angle=5.492507
x=-0.357469,y=0.147807,angle=-0.788850
x=-0.357137,y=0.147474,angle=5.494907
x=-0.356806,y=0.147141,angle=-0.787707
x=-0.356435,y=0.146769,angle=5.496678
x=-0.356064,y=0.146398,angle=-0.785307
x=-0.355692,y=0.146027,angle=5.499079
x=-0.355359,y=0.145696,angle=-0.783535
x=-0.355026,y=0.145364,angle=5.500221
x=-0.354692,y=0.145033,angle=-0.782393
x=-0.354359,y=0.144702,angle=5.501364
x=-0.353986,y=0.144332,angle=-0.780621
x=-0.353612,y=0.143963,angle=5.503764
x=-0.353278,y=0.143633,angle=-0.778850
x=-0.352904,y=0.143265,angle=5.505536
x=-0.352608,y=0.142974,angle=-0.776450
x=-0.352233,y=0.142607,angle=5.507936
x=-0.351857,y=0.142240,angle=-0.774049
x=-0.351482,y=0.141873,angle=5.510336
x=-0.351105,y=0.141507,angle=-0.771649
x=-0.350728,y=0.141141,angle=5.512737
x=-0.350351,y=0.140776,angle=-0.769248
x=-0.349974,y=0.140411,angle=5.515137
x=-0.349675,y=0.140123,angle=-0.766848
x=-0.349296,y=0.139760,angle=5.517538
x=-0.348957,y=0.139434,angle=-0.765076
x=-0.348578,y=0.139071,angle=5.519309
x=-0.348199,y=0.138708,angle=-0.762676
x=-0.347859,y=0.138384,angle=5.521080
x=-0.347518,y=0.138059,angle=-0.761534
x=-0.347138,y=0.137698,angle=5.522852
x=-0.346757,y=0.137336,angle=-0.759133
x=-0.346380,y=0.136978,angle=5.524052
x=-0.345999,y=0.136617,angle=-0.757933
x=-0.345657,y=0.136295,angle=5.527081
x=-0.345315,y=0.135973,angle=-0.755533
x=-0.344932,y=0.135613,angle=5.528852
x=-0.344549,y=0.135254,angle=-0.753133
x=-0.344206,y=0.134933,angle=5.530624
x=-0.343822,y=0.134574,angle=-0.751361
x=-0.343479,y=0.134254,angle=5.532395
x=-0.343094,y=0.133896,angle=-0.749590
x=-0.342710,y=0.133539,angle=5.534796
x=-0.342365,y=0.133219,angle=-0.747818
x=-0.342020,y=0.132900,angle=5.535938
x=-0.341635,y=0.132543,angle=-0.746047
x=-0.341249,y=0.132188,angle=5.538339
x=-0.340862,y=0.131832,angle=-0.743646
x=-0.340516,y=0.131515,angle=5.541368
x=-0.340169,y=0.131197,angle=-0.741247
x=-0.339781,y=0.130843,angle=5.543139
x=-0.339434,y=0.130527,angle=-0.739475
x=-0.339086,y=0.130210,angle=5.545539
x=-0.338697,y=0.129858,angle=-0.736446
x=-0.338308,y=0.129506,angle=5.547939
x=-0.337922,y=0.129157,angle=-0.735246
x=-0.337533,y=0.128805,angle=5.549139
x=-0.337224,y=0.128528,angle=-0.732846
x=-0.336834,y=0.128177,angle=5.551540
x=-0.336484,y=0.127863,angle=-0.731074
x=-0.336092,y=0.127513,angle=5.553311
x=-0.335705,y=0.127166,angle=-0.729874
x=-0.335313,y=0.126817,angle=5.554512
x=-0.334962,y=0.126504,angle=-0.726845
x=-0.334569,y=0.126156,angle=5.557540
x=-0.334180,y=0.125811,angle=-0.725645
x=-0.333828,y=0.125499,angle=5.558112
x=-0.333435,y=0.125151,angle=-0.723873
x=-0.333124,y=0.124877,angle=5.560512
x=-0.332730,y=0.124530,angle=-0.721473
x=-0.332376,y=0.124220,angle=5.562284
x=-0.331982,y=0.123874,angle=-0.719702
x=-0.331586,y=0.123528,angle=5.564684
x=-0.331232,y=0.123219,angle=-0.717930
x=-0.330878,y=0.122911,angle=5.567084
x=-0.330523,y=0.122602,angle=-0.715530
x=-0.330168,y=0.122295,angle=5.569484
x=-0.329733,y=0.121918,angle=-0.713073
x=-0.329335,y=0.121576,angle=5.571312
x=-0.328938,y=0.121233,angle=-0.710673
x=-0.328539,y=0.120891,angle=5.573713
x=-0.328182,y=0.120585,angle=-0.708901
x=-0.327826,y=0.120279,angle=5.574855
x=-0.327389,y=0.119905,angle=-0.707702
x=-0.327031,y=0.119600,angle=5.576055
x=-0.326673,y=0.119295,angle=-0.705302
x=-0.326315,y=0.118991,angle=5.578455
x=-0.325915,y=0.118651,angle=-0.703530
x=-0.325514,y=0.118312,angle=5.580855
x=-0.325113,y=0.117974,angle=-0.701130
x=-0.324754,y=0.117671,angle=5.582627
x=-0.324352,y=0.117333,angle=-0.699358
x=-0.323992,y=0.117031,angle=5.585656
x=-0.323631,y=0.116729,angle=-0.696958
x=-0.323228,y=0.116393,angle=5.587427
x=-0.322825,y=0.116056,angle=-0.694558
x=-0.322464,y=0.115756,angle=5.589199
x=-0.322060,y=0.115421,angle=-0.692787
x=-0.321698,y=0.115121,angle=5.590970
x=-0.321293,y=0.114786,angle=-0.691015
x=-0.320888,y=0.114452,angle=5.593370
x=-0.320526,y=0.114153,angle=-0.689244
x=-0.320163,y=0.113854,angle=5.594513
x=-0.319757,y=0.113521,angle=-0.687472
x=-0.319351,y=0.113188,angle=5.596913
x=-0.318987,y=0.112891,angle=-0.685701
x=-0.318623,y=0.112593,angle=5.598056
x=-0.318216,y=0.112262,angle=-0.683929
x=-0.317809,y=0.111931,angle=5.600456
x=-0.317444,y=0.111635,angle=-0.680900
x=-0.317118,y=0.111370,angle=5.602285
x=-0.316671,y=0.111009,angle=-0.680272
x=-0.316262,y=0.110679,angle=5.604114
x=-0.315939,y=0.110419,angle=-0.677872
x=-0.315572,y=0.110124,angle=5.605885
x=-0.315163,y=0.109796,angle=-0.676100
x=-0.314753,y=0.109467,angle=5.608285
x=-0.314343,y=0.109140,angle=-0.673700
x=-0.313932,y=0.108813,angle=5.610686
x=-0.313521,y=0.108486,angle=-0.671299
x=-0.313152,y=0.108195,angle=5.613715
x=-0.312745,y=0.107872,angle=-0.669471
x=-0.312333,y=0.107547,angle=5.614915
x=-0.311964,y=0.107256,angle=-0.667699
x=-0.311551,y=0.106931,angle=5.616686
x=-0.311138,y=0.106607,angle=-0.665299
x=-0.310768,y=0.106318,angle=5.619715
x=-0.310358,y=0.105997,angle=-0.663470
x=-0.309987,y=0.105708,angle=5.620286
x=-0.309573,y=0.105385,angle=-0.661699
x=-0.309202,y=0.105097,angle=5.623315
x=-0.308830,y=0.104809,angle=-0.659299
x=-0.308459,y=0.104522,angle=5.624458
x=-0.308043,y=0.104201,angle=-0.657527
x=-0.307588,y=0.103850,angle=5.626287
x=-0.307215,y=0.103563,angle=-0.655070
x=-0.306842,y=0.103277,angle=5.628686
x=-0.306469,y=0.102991,angle=-0.653928
x=-0.306052,y=0.102673,angle=5.630458
x=-0.305718,y=0.102417,angle=-0.652727
x=-0.305301,y=0.102099,angle=5.631658
x=-0.304927,y=0.101814,angle=-0.650956
x=-0.304509,y=0.101497,angle=5.633430
x=-0.304135,y=0.101213,angle=-0.649185
x=-0.303716,y=0.100896,angle=5.635201
x=-0.303297,y=0.100579,angle=-0.646784
x=-0.302878,y=0.100263,angle=5.637601
x=-0.302458,y=0.099948,angle=-0.644384
x=-0.302082,y=0.099666,angle=5.640630
x=-0.301661,y=0.099352,angle=-0.641355
x=-0.301240,y=0.099039,angle=5.643031
x=-0.300819,y=0.098726,angle=-0.638954
x=-0.300441,y=0.098446,angle=5.646060
x=-0.300063,y=0.098167,angle=-0.636555
x=-0.299685,y=0.097887,angle=5.647202
x=-0.299262,y=0.097576,angle=-0.634783
x=-0.298799,y=0.097235,angle=5.649031
x=-0.298420,y=0.096957,angle=-0.633583
x=-0.298041,y=0.096680,angle=5.651431
x=-0.297661,y=0.096402,angle=-0.631183
x=-0.297237,y=0.096093,angle=5.653202
x=-0.296772,y=0.095755,angle=-0.629355
x=-0.296392,y=0.095478,angle=5.654402
x=-0.296056,y=0.095234,angle=-0.627583
x=-0.295591,y=0.094897,angle=5.656230
x=-0.295210,y=0.094622,angle=-0.625126
x=-0.294788,y=0.094318,angle=5.658059
x=-0.294407,y=0.094043,angle=-0.624555
x=-0.293980,y=0.093736,angle=5.659831
x=-0.293599,y=0.093462,angle=-0.622783
x=-0.293261,y=0.093221,angle=5.661602
x=-0.292834,y=0.092915,angle=-0.620383
x=-0.292406,y=0.092611,angle=5.664002
x=-0.291979,y=0.092307,angle=-0.617983
x=-0.291595,y=0.092034,angle=5.665774
x=-0.291167,y=0.091731,angle=-0.616211
x=-0.290738,y=0.091428,angle=5.668174
x=-0.290309,y=0.091126,angle=-0.613811
x=-0.289925,y=0.090855,angle=5.669946
x=-0.289495,y=0.090554,angle=-0.612039
x=-0.289110,y=0.090284,angle=5.672975
x=-0.288679,y=0.089984,angle=-0.609011
x=-0.288248,y=0.089684,angle=5.675375
x=-0.287817,y=0.089385,angle=-0.606610
x=-0.287431,y=0.089117,angle=5.677146
x=-0.287044,y=0.088850,angle=-0.605468
x=-0.286612,y=0.088551,angle=5.678918
x=-0.286180,y=0.088254,angle=-0.603067
x=-0.285792,y=0.087987,angle=5.680689
x=-0.285359,y=0.087690,angle=-0.601296
x=-0.284972,y=0.087425,angle=5.682461
x=-0.284584,y=0.087160,angle=-0.598896
x=-0.284150,y=0.086864,angle=5.685490
x=-0.283715,y=0.086569,angle=-0.596495
x=-0.283326,y=0.086305,angle=5.687261
x=-0.282937,y=0.086042,angle=-0.595353
x=-0.282502,y=0.085748,angle=5.689033
x=-0.282067,y=0.085455,angle=-0.592953
x=-0.281677,y=0.085192,angle=5.690804
x=-0.281332,y=0.084961,angle=-0.591181
x=-0.280855,y=0.084641,angle=5.692633
x=-0.280418,y=0.084349,angle=-0.589352
x=-0.280027,y=0.084088,angle=5.694404
x=-0.279636,y=0.083827,angle=-0.588210
x=-0.279245,y=0.083567,angle=5.696804
x=-0.278807,y=0.083277,angle=-0.585181
x=-0.278369,y=0.082988,angle=5.699204
x=-0.277889,y=0.082671,angle=-0.583352
x=-0.277497,y=0.082413,angle=5.701662
x=-0.277104,y=0.082155,angle=-0.580952
x=-0.276623,y=0.081839,angle=5.702861
x=-0.276230,y=0.081582,angle=-0.579753
x=-0.275790,y=0.081295,angle=5.704633
x=-0.275396,y=0.081039,angle=-0.576724
x=-0.274956,y=0.080753,angle=5.707662
x=-0.274561,y=0.080497,angle=-0.574952
x=-0.274167,y=0.080242,angle=5.708804
x=-0.273726,y=0.079957,angle=-0.573181
x=-0.273242,y=0.079646,angle=5.710633
x=-0.272893,y=0.079421,angle=-0.571352
x=-0.272451,y=0.079138,angle=5.713033
x=-0.272009,y=0.078855,angle=-0.568952
x=-0.271613,y=0.078602,angle=5.714805
x=-0.271170,y=0.078320,angle=-0.567180
x=-0.270727,y=0.078038,angle=5.717205
x=-0.270330,y=0.077787,angle=-0.565409
x=-0.269933,y=0.077535,angle=5.718348
x=-0.269536,y=0.077284,angle=-0.563009
x=-0.269138,y=0.077034,angle=5.720747
x=-0.268651,y=0.076727,angle=-0.561809
x=-0.268253,y=0.076477,angle=5.721947
x=-0.267855,y=0.076228,angle=-0.559409
x=-0.267410,y=0.075950,angle=5.724976
x=-0.266922,y=0.075645,angle=-0.557581
x=-0.266523,y=0.075397,angle=5.726176
x=-0.266170,y=0.075178,angle=-0.555809
x=-0.265724,y=0.074901,angle=5.728576
x=-0.265277,y=0.074626,angle=-0.553409
x=-0.264830,y=0.074350,angle=5.730977
x=-0.264383,y=0.074075,angle=-0.551009
x=-0.263983,y=0.073829,angle=5.732748
x=-0.263535,y=0.073555,angle=-0.549237
x=-0.263134,y=0.073310,angle=5.734519
x=-0.262685,y=0.073037,angle=-0.547466
x=-0.262284,y=0.072793,angle=5.736291
x=-0.261835,y=0.072520,angle=-0.545694
x=-0.261433,y=0.072276,angle=5.738062
x=-0.261031,y=0.072033,angle=-0.543294
x=-0.260581,y=0.071763,angle=5.741091
x=-0.260131,y=0.071492,angle=-0.540894
x=-0.259728,y=0.071250,angle=5.742863
x=-0.259278,y=0.070981,angle=-0.539122
x=-0.258827,y=0.070712,angle=5.745263
x=-0.258423,y=0.070471,angle=-0.537351
x=-0.258019,y=0.070232,angle=5.747663
x=-0.257572,y=0.069966,angle=-0.535522
x=-0.257120,y=0.069699,angle=5.748863
x=-0.256715,y=0.069460,angle=-0.533751
x=-0.256263,y=0.069193,angle=5.750635
x=-0.255810,y=0.068927,angle=-0.531350

[43996173]

@43996173

运行micro_ros_agent日志

tingbo@tingbo-i7:~/chapt9/fishbot_ws$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888
[1714964344.546633] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1714964344.546982] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1714964354.054123] info     | Root.cpp           | create_client            | create                 | client_key: 0x17E096E3, session_id: 0x81
[1714964354.054266] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x17E096E3, address: 192.168.1.17:47138
[1714964354.093086] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x17E096E3, participant_id: 0x000(1)
[1714964354.398775] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x17E096E3, topic_id: 0x000(2), participant_id: 0x000(1)
[1714964354.412335] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x17E096E3, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1714964354.421320] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x17E096E3, datareader_id: 0x000(6), subscriber_id: 0x000(4)
[1714964445.911647] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x17E096E3
[1714964445.911742] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x17E096E3, address: 192.168.1.17:47138
[1714964445.911805] info     | Root.cpp           | create_client            | create                 | client_key: 0x73BC5BF6, session_id: 0x81
[1714964445.911834] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x73BC5BF6, address: 192.168.1.17:47138
[1714964445.911901] info     | SessionManager.hpp | establish_session        | session re-established | client_key: 0x73BC5BF6, address: 192.168.1.17:47138
[1714964445.927577] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x73BC5BF6, participant_id: 0x000(1)
[1714964445.940184] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x73BC5BF6, topic_id: 0x000(2), participant_id: 0x000(1)
[1714964445.957322] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x73BC5BF6, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1714964445.971723] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x73BC5BF6, datareader_id: 0x000(6), subscriber_id: 0x000(4)
^C[ros2run]: Interrupt
tingbo@tingbo-i7:~/chapt9/fishbot_ws$ ^C

[43996173]

@43996173
Kinematics.cpp

#include "Kinematics.h"

void Kinematics::set_motor_param(uint8_t id, float per_pulse_distance) {
    motor_param_[id].per_pulse_distance = per_pulse_distance;/*电机每个脉冲前进的距离*/
}
void Kinematics::set_wheel_distance(float wheel_distance) {
    wheel_distance_ = wheel_distance;

}
//传入电机编号id 返回该电机的速度
int16_t Kinematics::get_motor_speed(uint8_t id) {
    return motor_param_[id].motor_speed;
}
/**
 * @brief 更新电机速度和编码器数据
 * @param current_time 当前时间（单位：毫秒）
 * @param left_tick 左轮编码器读数
 * @param right_tick 右轮编码器读数
*/
void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick) {
    //计算出自上次更新以来经过的时间 dt
    uint32_t dt = current_time - last_update_time;
    last_update_time = current_time;
    //计算电机1和电机2的编码器读数变化量dtick1和dtick2
    int32_t dtick1 = left_tick - motor_param_[0].last_encoder_tick;
    int32_t dtick2 = right_tick - motor_param_[1].last_encoder_tick;
    motor_param_[0].last_encoder_tick = left_tick;
    motor_param_[1].last_encoder_tick = right_tick;
    //轮子速度计算
    motor_param_[0].motor_speed = 
        float(dtick1 * motor_param_[0].per_pulse_distance) / dt * 1000;
    motor_param_[1].motor_speed = 
        float(dtick2 * motor_param_[1].per_pulse_distance) / dt * 1000;
    //更新里程计信息
    update_odom(dt);
}
/**
 * @brief 正运动学计算，将左右轮的速度转换为线速度和角速度
 * @param left_speed 左轮速度（单位：毫米/秒）
 * @param right_speed 右轮速度
 * @param[out] out_linear_speed 线速度（单位：毫米/秒）
 * @param[out] out_angle_speed 角速度（单位：弧度/秒）
*/
void Kinematics::kinematic_forward(float left_speed, float right_speed,
                                    float &out_linear_speed,
                                    float &out_angle_speed) {
    // 两轮转速之和除以2
    out_linear_speed = (right_speed + left_speed) / 2.0;
    //两轮转速之差除以轮距
    out_angle_speed = (right_speed - left_speed) / wheel_distance_; 
}

/**
 * @brief 逆运动学计算，将线速度和角速度转换为左右轮速度
 * @param linear_speed 线速度（单位：毫米/秒）
 * @param angle_speed 角速度 （单位：弧度/秒）
 * @param[out] out_left_speed 左轮速度（单位：毫米/秒）
 * @param[out] out_right_speed 右轮速度
*/
void Kinematics::kinematic_inverse(float linear_speed, float angle_speed,
                                    float &out_left_speed,
                                    float &out_right_speed) {
    out_left_speed = linear_speed - (angle_speed * wheel_distance_) / 2.0;
    out_right_speed = linear_speed + (angle_speed * wheel_distance_) / 2.0;
}

odom_t &Kinematics::get_odom() { return odom_; }


//用于将角度转换到-Π到Π的范围内
void Kinematics::TransAngleInPI(float angle, float &out_angle) {
    //如果angle大于Π，则将out_angle减去2Π
    if (angle > PI) {
        out_angle -= 2 * PI;
    }
    else if (angle < PI) {
        out_angle += 2 * PI;

    }

}

void Kinematics::update_odom(uint16_t dt) {
    //毫秒转为秒
    float dt_s = (float)dt / 1000;
    //运动学正解，计算机器人的线速度和角速度
    this->kinematic_forward(motor_param_[0].motor_speed,
    motor_param_[1].motor_speed, odom_.linear_speed, odom_.angle_speed);
    //转换线速度单位（mm/s->m/s）
    odom_.linear_speed = odom_.linear_speed / 1000;

    //计算当前角度
    odom_.angle += odom_.angle_speed * dt_s;
    //将角度值odom_yaw 转换到-Π和Π之间
    Kinematics::TransAngleInPI(odom_.angle, odom_.angle);
    //计算机器人移动距离和在两轴上的分量并进行累积
    float delta_distance = odom_.linear_speed * dt_s;
    odom_.x += delta_distance * std::cos(odom_.angle);
    odom_.y += delta_distance * std::sin(odom_.angle);
}

[43996173]

Kinematics.h代码：

#ifndef __KINEMATICS_H__ /*防止头文件被多次包含*/
#define __KINEMATICS_H__

#include <Arduino.h> /*包含Arduino核心库*/

//定义一个结构体用于存储电机参数
typedef struct 
{
    float per_pulse_distance; /*单个脉冲对用轮子的前进距离*/
    int16_t motor_speed;      /*当前电机前进速度mm/s，计算时使用*/
    int16_t last_encoder_tick; /*上次电机编码器读数*/

} motor_param_t;

typedef struct
{
    float x; /*位置x*/
    float y; /*位置y*/
    float angle; /*角度*/
    float linear_speed; /*线速度*/
    float angle_speed; /*角速度*/
} odom_t;
/*定义一个类用于处理机器人运动学*/
class Kinematics
{
public:
    /*构造函数，默认实现*/
    Kinematics() = default;
    /*构造函数，默认实现*/
    ~Kinematics() = default;

    /*设置电机参数，包括编号和每个脉冲对应的轮子前进距离*/
    void set_motor_param(uint8_t id, float per_pulse_distance);
    /*设置轮子间距*/
    void set_wheel_distance(float wheel_distance);
    /*逆运动学计算，将线速度和角速度转化为左右轮的速度*/
    void kinematic_inverse(float linear_speed, float angle_speed,
                            float &out_left_speed, float &out_right_speed);
    /*正运动学计算，将左右轮子的速度转换为线速度和角速度*/
    void kinematic_forward(float left_speed, float right_speed,
                            float &out_linear_speed, float &out_angle_speed);
    /*更新电机速度和编码器数据*/
    void update_motor_speed(uint64_t current_time,int32_t left_tick,
                            int32_t right_tick);
    /*获取电机速度*/
    int16_t get_motor_speed(uint8_t id);
    /*更新里程计数据*/
    void update_odom(uint16_t dt);
    /*获取里程计数据*/
    odom_t &get_odom();
    /*用于将角速度转换到-Π到Π的范围内*/
    static void TransAngleInPI(float angle, float &out_angle);

private:
    odom_t odom_; /*存储里程计信息*/
    motor_param_t motor_param_[2]; /*存储两个电机的参数*/
    uint64_t last_update_time;  /*上次更新数据的时间，单位ms*/
    float wheel_distance_;    /*轮子间距*/
};


#endif // __KINEMATICS_H__

[43996173]

PidController..cpp代码：

#include "PidController.h"
#include "Arduino.h"

PidController::PidController(float kp, float ki, float kd)
{
    reset(); //初始化控制器
    update_pid(kp, ki, kd);  //更新PID参数

}

float PidController::update(float current)
{
    // 计算误差及其变化率
    float error = target_ - current;   //计算误差
    derror_ = error_last_ - error;     //误差变化率
    error_last_ = error;               //更新上一次误差为当前误差

    //计算积分并进行积分限制
    error_sum_ += error;
    if (error_sum_ > intergral_up_)
        error_sum_ = intergral_up_;
    if (error_sum_ < -1 * intergral_up_)
        error_sum_ = -1 * intergral_up_;
    
    //计算控制输出值
    float output = kp_ * error + ki_ * error_sum_ + kd_ * derror_;
    
    //控制输出限幅
    if (output > out_max_)
        output = out_max_;
    if (output < out_min_)
        output = out_min_;
    
    return output;
}

void PidController::update_target(float target)
{
    target_ = target; // 更新控制目标值
}

void PidController::update_pid(float kp, float ki, float kd)
{
    reset(); // 重置控制器状态
    kp_ = kp; //更新比例项系数
    ki_ = ki; //更新积分项系数
    kd_ = kd; //更新微分项系数
}
void PidController::reset()
{
    //重置控制器状态
    target_ = 0.0f; //控制目标值
    out_min_ = 0.0f; //控制输出最小值
    out_max_ = 0.0f; //控制输出最大值
    kp_ = 0.0f;  //比例项系数
    ki_ = 0.0f;  //积分项系数
    kd_ = 0.0f;  //微分项系数
    error_sum_ = 0.0f; //误差累记值
    error_last_ = 0.0f; //上一次的误差值
    derror_ = 0.0f; //误差变化率
} 

void PidController::out_limit(float out_min, float out_max)
{
    out_min_ = out_min; //控制输出最小值
    out_max_ = out_max; //控制输出最大值
}

[43996173]

PidController.h代码：

#ifndef __PIDCONTROLLER_H__ //如果没有定义__PIDCONTROLLER_H__
#define __PIDCONTROLLER_H__ //定义__PIDCONTROLLER_H__

class PidController
{ //定义一个PID控制类
public:
    PidController() = default;        //默认构造函数
    PidController(float kp, float ki, float kd); //构造函数，传入kp、ki、kd

private:
    float target_;   //目标值
    float out_min_;  //输出下限
    float out_max_;  //输出上限
    float kp_;       //比例系数
    float ki_;       //积分系数
    float kd_;       //微分系数
    //pid
    float error_sum_;        //误差积累和
    float derror_;           //误差变化率
    float error_last_;       //上一次误差
    float error_pre_;        //上上次误差
    float intergral_up_ = 2500;  //积分上限

public:
    float update(float current);                             //提供当前值返回下次输出值
    void update_target(float target);                        //更新目标值
    void update_pid(float kp, float ki, float kd);           //更新PID系数
    void reset();                                            //重置PID控制器
    void out_limit(float out_min, float out_max);            //设置输出限制

};

#endif //__PIDCONTROLLER_H__//结束条件

[43996173]

platformio.ini代码：

; PlatformIO Project Configuration File
;
;   Build options: build flags, source filter
;   Upload options: custom upload port, speed and extra flags
;   Library options: dependencies, extra library storages
;   Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html

[env:featheresp32]
platform = espressif32
board = featheresp32
framework = arduino
board_microros_transport = wifi
lib_deps = https://github.com/fishros/Esp32McpwmMotor.git
            https://github.com/fishros/Esp32PcntEncoder.git
            https://gitee.com/ohhuo/micro_ros_platformio.git

[小鱼]

@43996173 你的角度变化为什么不是连续的，不是5.3 就是 -0.9