File content as of revision 0:74030c9b6949:

/***************************< 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;
}

/*--------------------------------------------------------------------*/