worasuchad haomachai
base controller of ackermann autonomous car
Dependencies: QEI PID Motor BNO055 LamborSteering ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:74030c9b6949
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 19 07:14:32 2019 +0000
@@ -0,0 +1,435 @@
+
+/***************************< File comment >***************************/
+/* project: Lamborghini's base control */
+/* project description : Mobile Robot */
+/*--------------------------------------------------------------------*/
+/* version : 1.0 */
+/* board : NUCLEO-F746ZG */
+/* detail : DC motor position control with potentiometer */
+/*--------------------------------------------------------------------*/
+/* create : 14/12/2018 */
+/* programmer : Worasuchad Haomachai */
+/**********************************************************************/
+
+/*--------------------------------------------------------------------*/
+/* Include file */
+/*--------------------------------------------------------------------*/
+#include "mbed.h"
+#include "LamborSteer.h"
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+#include "BNO055.h"
+#include <ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Empty.h>
+#include <geometry_msgs/Twist.h>
+
+Timer timer1;
+Thread thread1; //Steering thread
+Thread thread2; //Drive thread
+Serial pc(USBTX, USBRX); //Serial Port
+Serial mc(PD_5, PD_6); //Tx Rx Serial Port
+BNO055 imu(I2C_SDA,I2C_SCL);
+ros::NodeHandle nh;
+
+/* Steering */
+LamborSteer lamborSteer(PE_15, PE_14, PE_12, A4, 0); //InA, InB, PWM, Potentiometer, Offset
+InterruptIn mybutton1(D13);
+InterruptIn mybutton2(D12);
+servo_status current_status;
+
+/* Drive */
+Motor leftMotor(D6, D7, D8); //pwm, inA, inB
+Motor rightMotor(D9, D10, D11); //pwm, inB, inA
+QEI leftQei(D2, D3, NC, 200); //chanA, chanB, index, ppr (19600 = 200*98)
+QEI rightQei(D4, D5, NC, 200); //chanB, chanA, index, ppr
+PID leftPid(0.4312, 0.01, 0.0, 0.01); //Kc, Ti, Td
+PID rightPid(0.4312, 0.01, 0.0, 0.01); //Kc, Ti, Td
+
+/* Sonic */
+AnalogIn pinL(A2);
+AnalogIn pinR(A1);
+
+/* LED */
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+/*--------------------------------------------------------------------*/
+/* Global variable */
+/*--------------------------------------------------------------------*/
+int angle;
+int forward = 1;
+bool cmDrive = true;
+char cmSteer;
+
+/* Drive */
+int leftPulses = 0; //How far the left wheel has travelled.
+int leftPrevPulses = 0; //The previous reading of how far the left wheel has travelled.
+float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second.
+int rightPulses = 0; //How far the right wheel has travelled.
+int rightPrevPulses = 0; //The previous reading of how far the right wheelhas travelled.
+float rightVelocity = 0.0; //The velocity of the right wheel in pulses persecond.
+int numTick = 0;
+int norTick = 0;
+
+/* ros */
+float g_req_linear_vel_x = 0;
+float g_req_linear_vel_y = 0;
+float g_req_angular_vel_z = 0;
+float servo_steering_angle;
+float delta_st;
+
+/* Timer variable */
+uint32_t watchTimer;
+uint32_t pubTimer;
+
+// ros
+uint32_t g_prev_command_time;
+uint32_t prev_control_time;
+#define MAX_STEERING_ANGLE 1.6 // max steering angle. This only applies to Ackermann steering
+#define PI 3.14
+
+// ultrasonic
+float voltL, voltR, inches, mm, cmL, cmR, cmM;
+
+// IMU
+float yaw_angle = 0.0;
+float init_yaw = 0.0;
+bool yawFirst = true;
+float change_yaw = 0.0;
+float cYaw = 0.0;
+/*--------------------------------------------------------------------*/
+/* prototype fun */
+/*--------------------------------------------------------------------*/
+void pressed();
+void watchSteer();
+void Drive();
+void Ultrasonic();
+void IMU();
+
+//ros
+void commandCallback(const geometry_msgs::Twist& cmd_msg);
+void moveBase();
+float steer(float steering_angle);
+float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);
+
+ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
+
+std_msgs::Float64 Tick;
+ros::Publisher Encoder("encTick", &Tick);
+
+std_msgs::Float64 ackermann;
+ros::Publisher Steer("steerPub", &ackermann);
+
+std_msgs::Float64 yaw;
+ros::Publisher pub_yaw("eulerYaw", &yaw);
+/*--------------------------------------------------------------------*/
+/* main */
+/*--------------------------------------------------------------------*/
+int main()
+{
+ pc.baud(115200);
+ mc.baud(115200);
+ nh.initNode();
+ nh.subscribe(cmd_sub);
+ nh.advertise(Encoder);
+ nh.advertise(Steer);
+ nh.advertise(pub_yaw);
+
+ /* IMU reset */
+ imu.reset();
+
+ /* Steering */
+ mybutton1.fall(&pressed);
+ mybutton2.fall(&pressed);
+ current_status = lamborSteer.read();
+ angle = current_status.current_angle;
+ lamborSteer.write(angle);
+
+ /* Drive */
+ leftMotor.period(0.00005); //Set motor PWM periods to 20KHz.
+ rightMotor.period(0.00005);
+ leftPid.setInputLimits(0, 50000);
+ leftPid.setOutputLimits(0.0, 1.0);
+ leftPid.setMode(AUTO_MODE);
+ rightPid.setInputLimits(0, 50000);
+ rightPid.setOutputLimits(0.0, 1.0);
+ rightPid.setMode(AUTO_MODE);
+ //Velocity to mantain in pulses per second.
+ leftPid.setSetPoint(19600);
+ rightPid.setSetPoint(19600);
+
+ timer1.start(); // start timer counting
+ thread1.start(watchSteer); // Steering thread start
+ thread2.start(Drive); // Drive thread start
+
+ /* IMU Check */
+ if (!imu.check())
+ {
+ while (true)
+ {
+ led3 = !led3;
+ wait(0.1);
+ }
+ }
+
+ prev_control_time = timer1.read_ms();
+ while(1)
+ {
+ IMU(); // IMU func
+ if ((timer1.read_ms() - prev_control_time) >= 1) // read time in 1 ms
+ {
+ moveBase();
+ //mc.printf("%.3f\t\t", g_req_linear_vel_x);
+ //mc.printf("%.3f\t\t", g_req_angular_vel_z);
+ //mc.printf("%.3f\n\r", servo_steering_angle*2);
+
+ if(change_yaw < -5 && change_yaw > -300)
+ {
+ cYaw = 80;
+ }
+ else if(change_yaw < -300)
+ {
+ cYaw = -80;
+ }
+ else
+ {
+ cYaw = 0;
+ }
+
+ lamborSteer.write(servo_steering_angle*2 + 80 + cYaw);
+ forward = g_req_linear_vel_x/2;
+
+ prev_control_time = timer1.read_ms();
+ }
+ //IMU(); // IMU func
+ Ultrasonic(); // Ultrasonic func
+ nh.spinOnce();
+ wait_ms(1);
+ }
+}
+
+/*--------------------------------------------------------------------*/
+/* IMU */
+/*--------------------------------------------------------------------*/
+void IMU()
+{
+ imu.setmode(OPERATION_MODE_NDOF);
+ imu.get_calib();
+ imu.get_angles();
+ imu.get_temp();
+ imu.get_quat();
+
+ if(yawFirst)
+ {
+ init_yaw = imu.euler.yaw;
+ yawFirst = false;
+ }
+
+ yaw_angle = imu.euler.yaw;
+ change_yaw = init_yaw - yaw_angle;
+ yaw.data = yaw_angle;
+ pub_yaw.publish(&yaw);
+ //pc.printf("Temperature\t%d\n", imu.temperature);
+ //pc.printf("Quaternion: w:%f\tx:%f\ty:%f\tz:%f\n", imu.quat.w, imu.quat.x, imu.quat.y, imu.quat.z);
+ //pc.printf("%u\t roll:%5.1f\t pitch:%5.1f\t yaw:%5.1f\r\n",imu.calib,imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
+}
+
+/*--------------------------------------------------------------------*/
+/* Ultrasonic */
+/*--------------------------------------------------------------------*/
+void Ultrasonic()
+{
+ mc.printf("sonic!");
+ voltL = pinL;
+ voltR = pinR;
+ cmL = voltL * 2000; //Takes bit count and converts it to mm
+ cmR = voltR * 2000;
+ //mc.printf("Distance L = %.3f cm\t\t", cmL);
+ //mc.printf("Distance R = %.3f cm\n\r", cmR);
+
+ if (cmL < 80 || cmR < 80 )
+ {
+ led1 = 1 ; // LED is ON
+ led2 = 1 ;
+ //led3 = 1 ;
+
+ forward = 0;
+ //leftMotor.brake();
+ //rightMotor.brake();
+ }
+ else
+ {
+ led1 = 0 ; // LED is OFF
+ led2 = 0 ;
+ //led3 = 0 ;
+ }
+ //wait(0.1);
+}
+
+/*--------------------------------------------------------------------*/
+/* checkST */
+/*--------------------------------------------------------------------*/
+
+void watchSteer()
+{
+ watchTimer = timer1.read_ms();
+ pubTimer = timer1.read_ms();
+ while(1)
+ {
+ //lamborSteer.timer_int_routine();
+ if ((timer1.read_ms() - watchTimer) >= 10) // read time in 10 ms
+ {
+ current_status = lamborSteer.read();
+ //mc.printf("%d\t\t", current_status.current_angle);
+ //mc.printf("%d\t\t", current_status.wanted_angle);
+ //mc.printf("%d\n\r", current_status.speed);
+ lamborSteer.timer_int_routine();
+
+ /*
+ if(current_status.current_angle < 200)
+ {
+ delta_st = -( (-PI * current_status.current_angle / 400) + (PI / 2) );
+ }
+ else if(current_status.current_angle >= 200)
+ {
+ delta_st = current_status.current_angle - 200;
+ delta_st = PI*delta_st / 400;
+ }
+
+ ackermann.data = delta_st;
+ Steer.publish( &ackermann );
+ */
+ watchTimer = timer1.read_ms();
+ }
+
+ /***********************/
+ if((timer1.read_ms() - pubTimer) >= 100) // read time in 100 ms
+ {
+ if(current_status.current_angle < 200)
+ {
+ delta_st = -( (-PI * current_status.current_angle / 400) + (PI / 2) );
+ }
+ else if(current_status.current_angle >= 200)
+ {
+ delta_st = current_status.current_angle - 200;
+ delta_st = PI*delta_st / 400;
+ }
+
+ ackermann.data = delta_st;
+ Steer.publish( &ackermann );
+
+ pubTimer = timer1.read_ms();
+ }
+ /***********************/
+ }
+}
+
+/*--------------------------------------------------------------------*/
+/* Drive */
+/*--------------------------------------------------------------------*/
+void Drive()
+{
+ while(1)
+ {
+ if(cmDrive) // Drive if cmDrive = true
+ {
+ leftPulses = leftQei.getPulses();
+ leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
+ leftPrevPulses = leftPulses;
+ leftPid.setProcessValue(fabs(leftVelocity));
+ leftMotor.speed(forward*leftPid.compute());
+
+ rightPulses = rightQei.getPulses();
+ rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
+ norTick = rightPulses - rightPrevPulses; // normarlize tick
+ rightPrevPulses = rightPulses;
+ rightPid.setProcessValue(fabs(rightVelocity));
+ rightMotor.speed(forward*rightPid.compute());
+
+ if( norTick >= 196) // 100 tick-per-r
+ {
+ numTick = numTick + 1;
+ Tick.data = numTick;
+ Encoder.publish( &Tick );
+ norTick = 0;
+ }
+ wait(0.01);
+ }
+ else
+ {
+ leftMotor.brake();
+ rightMotor.brake();
+ }
+ }
+}
+
+/*--------------------------------------------------------------------*/
+/* pressed */
+/*--------------------------------------------------------------------*/
+void pressed()
+{
+ lamborSteer.stop();
+}
+
+/*--------------------------------------------------------------------*/
+/* ros */
+/*--------------------------------------------------------------------*/
+void commandCallback(const geometry_msgs::Twist& cmd_msg)
+{
+ //callback function every time linear and angular speed is received from 'cmd_vel' topic
+ //this callback function receives cmd_msg object where linear and angular speed are stored
+ g_req_linear_vel_x = cmd_msg.linear.x;
+ g_req_linear_vel_y = cmd_msg.linear.y;
+ g_req_angular_vel_z = cmd_msg.angular.z;
+
+ //pc.printf("%.3f\t\t", g_req_linear_vel_x);
+ //pc.printf("%.3f\t\t", g_req_linear_vel_y);
+ //pc.printf("%.3f\n\r", g_req_angular_vel_z);
+ g_prev_command_time = timer1.read_ms();;
+}
+
+void moveBase()
+{
+ float current_steering_angle;
+ current_steering_angle = steer(g_req_angular_vel_z);
+ //pc.printf("%.3f\t\t", g_req_linear_vel_x);
+ //pc.printf("%.3f\t\t", g_req_linear_vel_y);
+ //pc.printf("%.3f\t\t", g_req_angular_vel_z);
+ //pc.printf("%.3f\n\r", current_steering_angle);
+}
+
+float steer(float steering_angle)
+{
+ //steering function for ACKERMANN base
+ //float servo_steering_angle;
+
+ //steering_angle = constrain(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE);
+ servo_steering_angle = mapFloat(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE, 0, PI) * (180 / PI);
+
+ if(servo_steering_angle < 0)
+ {
+ servo_steering_angle = 0;
+ }
+ else if(servo_steering_angle > 200)
+ {
+ servo_steering_angle = 200;
+ }
+
+ //servo_steering_angle = mapFloat(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE, PI, 0) * (180 / PI);
+
+ //steering_servo.write(servo_steering_angle);
+
+ //return steering_angle;
+ return servo_steering_angle;
+}
+
+float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
+{
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+/*--------------------------------------------------------------------*/
\ No newline at end of file