worasuchad haomachai / Mbed OS baseControl_ackermannCar

base controller of ackermann autonomous car

Dependencies:   QEI PID Motor BNO055 LamborSteering ros_lib_kinetic

main.cpp@0:74030c9b6949, 2019-02-19 (annotated)

Committer:
worasuchad
Date:
Tue Feb 19 07:14:32 2019 +0000
Revision:
0:74030c9b6949
base controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 0:74030c9b6949 1
worasuchad 0:74030c9b6949 2 /***************************< File comment >***************************/
worasuchad 0:74030c9b6949 3 /* project: Lamborghini's base control */
worasuchad 0:74030c9b6949 4 /* project description : Mobile Robot */
worasuchad 0:74030c9b6949 5 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 6 /* version : 1.0 */
worasuchad 0:74030c9b6949 7 /* board : NUCLEO-F746ZG */
worasuchad 0:74030c9b6949 8 /* detail : DC motor position control with potentiometer */
worasuchad 0:74030c9b6949 9 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 10 /* create : 14/12/2018 */
worasuchad 0:74030c9b6949 11 /* programmer : Worasuchad Haomachai */
worasuchad 0:74030c9b6949 12 /**********************************************************************/
worasuchad 0:74030c9b6949 13
worasuchad 0:74030c9b6949 14 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 15 /* Include file */
worasuchad 0:74030c9b6949 16 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 17 #include "mbed.h"
worasuchad 0:74030c9b6949 18 #include "LamborSteer.h"
worasuchad 0:74030c9b6949 19 #include "Motor.h"
worasuchad 0:74030c9b6949 20 #include "QEI.h"
worasuchad 0:74030c9b6949 21 #include "PID.h"
worasuchad 0:74030c9b6949 22 #include "BNO055.h"
worasuchad 0:74030c9b6949 23 #include <ros.h>
worasuchad 0:74030c9b6949 24 #include <std_msgs/Float64.h>
worasuchad 0:74030c9b6949 25 #include <std_msgs/Int32.h>
worasuchad 0:74030c9b6949 26 #include <std_msgs/Empty.h>
worasuchad 0:74030c9b6949 27 #include <geometry_msgs/Twist.h>
worasuchad 0:74030c9b6949 28
worasuchad 0:74030c9b6949 29 Timer timer1;
worasuchad 0:74030c9b6949 30 Thread thread1; //Steering thread
worasuchad 0:74030c9b6949 31 Thread thread2; //Drive thread
worasuchad 0:74030c9b6949 32 Serial pc(USBTX, USBRX); //Serial Port
worasuchad 0:74030c9b6949 33 Serial mc(PD_5, PD_6); //Tx Rx Serial Port
worasuchad 0:74030c9b6949 34 BNO055 imu(I2C_SDA,I2C_SCL);
worasuchad 0:74030c9b6949 35 ros::NodeHandle nh;
worasuchad 0:74030c9b6949 36
worasuchad 0:74030c9b6949 37 /* Steering */
worasuchad 0:74030c9b6949 38 LamborSteer lamborSteer(PE_15, PE_14, PE_12, A4, 0); //InA, InB, PWM, Potentiometer, Offset
worasuchad 0:74030c9b6949 39 InterruptIn mybutton1(D13);
worasuchad 0:74030c9b6949 40 InterruptIn mybutton2(D12);
worasuchad 0:74030c9b6949 41 servo_status current_status;
worasuchad 0:74030c9b6949 42
worasuchad 0:74030c9b6949 43 /* Drive */
worasuchad 0:74030c9b6949 44 Motor leftMotor(D6, D7, D8); //pwm, inA, inB
worasuchad 0:74030c9b6949 45 Motor rightMotor(D9, D10, D11); //pwm, inB, inA
worasuchad 0:74030c9b6949 46 QEI leftQei(D2, D3, NC, 200); //chanA, chanB, index, ppr (19600 = 200*98)
worasuchad 0:74030c9b6949 47 QEI rightQei(D4, D5, NC, 200); //chanB, chanA, index, ppr
worasuchad 0:74030c9b6949 48 PID leftPid(0.4312, 0.01, 0.0, 0.01); //Kc, Ti, Td
worasuchad 0:74030c9b6949 49 PID rightPid(0.4312, 0.01, 0.0, 0.01); //Kc, Ti, Td
worasuchad 0:74030c9b6949 50
worasuchad 0:74030c9b6949 51 /* Sonic */
worasuchad 0:74030c9b6949 52 AnalogIn pinL(A2);
worasuchad 0:74030c9b6949 53 AnalogIn pinR(A1);
worasuchad 0:74030c9b6949 54
worasuchad 0:74030c9b6949 55 /* LED */
worasuchad 0:74030c9b6949 56 DigitalOut led1(LED1);
worasuchad 0:74030c9b6949 57 DigitalOut led2(LED2);
worasuchad 0:74030c9b6949 58 DigitalOut led3(LED3);
worasuchad 0:74030c9b6949 59
worasuchad 0:74030c9b6949 60 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 61 /* Global variable */
worasuchad 0:74030c9b6949 62 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 63 int angle;
worasuchad 0:74030c9b6949 64 int forward = 1;
worasuchad 0:74030c9b6949 65 bool cmDrive = true;
worasuchad 0:74030c9b6949 66 char cmSteer;
worasuchad 0:74030c9b6949 67
worasuchad 0:74030c9b6949 68 /* Drive */
worasuchad 0:74030c9b6949 69 int leftPulses = 0; //How far the left wheel has travelled.
worasuchad 0:74030c9b6949 70 int leftPrevPulses = 0; //The previous reading of how far the left wheel has travelled.
worasuchad 0:74030c9b6949 71 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second.
worasuchad 0:74030c9b6949 72 int rightPulses = 0; //How far the right wheel has travelled.
worasuchad 0:74030c9b6949 73 int rightPrevPulses = 0; //The previous reading of how far the right wheelhas travelled.
worasuchad 0:74030c9b6949 74 float rightVelocity = 0.0; //The velocity of the right wheel in pulses persecond.
worasuchad 0:74030c9b6949 75 int numTick = 0;
worasuchad 0:74030c9b6949 76 int norTick = 0;
worasuchad 0:74030c9b6949 77
worasuchad 0:74030c9b6949 78 /* ros */
worasuchad 0:74030c9b6949 79 float g_req_linear_vel_x = 0;
worasuchad 0:74030c9b6949 80 float g_req_linear_vel_y = 0;
worasuchad 0:74030c9b6949 81 float g_req_angular_vel_z = 0;
worasuchad 0:74030c9b6949 82 float servo_steering_angle;
worasuchad 0:74030c9b6949 83 float delta_st;
worasuchad 0:74030c9b6949 84
worasuchad 0:74030c9b6949 85 /* Timer variable */
worasuchad 0:74030c9b6949 86 uint32_t watchTimer;
worasuchad 0:74030c9b6949 87 uint32_t pubTimer;
worasuchad 0:74030c9b6949 88
worasuchad 0:74030c9b6949 89 // ros
worasuchad 0:74030c9b6949 90 uint32_t g_prev_command_time;
worasuchad 0:74030c9b6949 91 uint32_t prev_control_time;
worasuchad 0:74030c9b6949 92 #define MAX_STEERING_ANGLE 1.6 // max steering angle. This only applies to Ackermann steering
worasuchad 0:74030c9b6949 93 #define PI 3.14
worasuchad 0:74030c9b6949 94
worasuchad 0:74030c9b6949 95 // ultrasonic
worasuchad 0:74030c9b6949 96 float voltL, voltR, inches, mm, cmL, cmR, cmM;
worasuchad 0:74030c9b6949 97
worasuchad 0:74030c9b6949 98 // IMU
worasuchad 0:74030c9b6949 99 float yaw_angle = 0.0;
worasuchad 0:74030c9b6949 100 float init_yaw = 0.0;
worasuchad 0:74030c9b6949 101 bool yawFirst = true;
worasuchad 0:74030c9b6949 102 float change_yaw = 0.0;
worasuchad 0:74030c9b6949 103 float cYaw = 0.0;
worasuchad 0:74030c9b6949 104 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 105 /* prototype fun */
worasuchad 0:74030c9b6949 106 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 107 void pressed();
worasuchad 0:74030c9b6949 108 void watchSteer();
worasuchad 0:74030c9b6949 109 void Drive();
worasuchad 0:74030c9b6949 110 void Ultrasonic();
worasuchad 0:74030c9b6949 111 void IMU();
worasuchad 0:74030c9b6949 112
worasuchad 0:74030c9b6949 113 //ros
worasuchad 0:74030c9b6949 114 void commandCallback(const geometry_msgs::Twist& cmd_msg);
worasuchad 0:74030c9b6949 115 void moveBase();
worasuchad 0:74030c9b6949 116 float steer(float steering_angle);
worasuchad 0:74030c9b6949 117 float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);
worasuchad 0:74030c9b6949 118
worasuchad 0:74030c9b6949 119 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
worasuchad 0:74030c9b6949 120
worasuchad 0:74030c9b6949 121 std_msgs::Float64 Tick;
worasuchad 0:74030c9b6949 122 ros::Publisher Encoder("encTick", &Tick);
worasuchad 0:74030c9b6949 123
worasuchad 0:74030c9b6949 124 std_msgs::Float64 ackermann;
worasuchad 0:74030c9b6949 125 ros::Publisher Steer("steerPub", &ackermann);
worasuchad 0:74030c9b6949 126
worasuchad 0:74030c9b6949 127 std_msgs::Float64 yaw;
worasuchad 0:74030c9b6949 128 ros::Publisher pub_yaw("eulerYaw", &yaw);
worasuchad 0:74030c9b6949 129 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 130 /* main */
worasuchad 0:74030c9b6949 131 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 132 int main()
worasuchad 0:74030c9b6949 133 {
worasuchad 0:74030c9b6949 134 pc.baud(115200);
worasuchad 0:74030c9b6949 135 mc.baud(115200);
worasuchad 0:74030c9b6949 136 nh.initNode();
worasuchad 0:74030c9b6949 137 nh.subscribe(cmd_sub);
worasuchad 0:74030c9b6949 138 nh.advertise(Encoder);
worasuchad 0:74030c9b6949 139 nh.advertise(Steer);
worasuchad 0:74030c9b6949 140 nh.advertise(pub_yaw);
worasuchad 0:74030c9b6949 141
worasuchad 0:74030c9b6949 142 /* IMU reset */
worasuchad 0:74030c9b6949 143 imu.reset();
worasuchad 0:74030c9b6949 144
worasuchad 0:74030c9b6949 145 /* Steering */
worasuchad 0:74030c9b6949 146 mybutton1.fall(&pressed);
worasuchad 0:74030c9b6949 147 mybutton2.fall(&pressed);
worasuchad 0:74030c9b6949 148 current_status = lamborSteer.read();
worasuchad 0:74030c9b6949 149 angle = current_status.current_angle;
worasuchad 0:74030c9b6949 150 lamborSteer.write(angle);
worasuchad 0:74030c9b6949 151
worasuchad 0:74030c9b6949 152 /* Drive */
worasuchad 0:74030c9b6949 153 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz.
worasuchad 0:74030c9b6949 154 rightMotor.period(0.00005);
worasuchad 0:74030c9b6949 155 leftPid.setInputLimits(0, 50000);
worasuchad 0:74030c9b6949 156 leftPid.setOutputLimits(0.0, 1.0);
worasuchad 0:74030c9b6949 157 leftPid.setMode(AUTO_MODE);
worasuchad 0:74030c9b6949 158 rightPid.setInputLimits(0, 50000);
worasuchad 0:74030c9b6949 159 rightPid.setOutputLimits(0.0, 1.0);
worasuchad 0:74030c9b6949 160 rightPid.setMode(AUTO_MODE);
worasuchad 0:74030c9b6949 161 //Velocity to mantain in pulses per second.
worasuchad 0:74030c9b6949 162 leftPid.setSetPoint(19600);
worasuchad 0:74030c9b6949 163 rightPid.setSetPoint(19600);
worasuchad 0:74030c9b6949 164
worasuchad 0:74030c9b6949 165 timer1.start(); // start timer counting
worasuchad 0:74030c9b6949 166 thread1.start(watchSteer); // Steering thread start
worasuchad 0:74030c9b6949 167 thread2.start(Drive); // Drive thread start
worasuchad 0:74030c9b6949 168
worasuchad 0:74030c9b6949 169 /* IMU Check */
worasuchad 0:74030c9b6949 170 if (!imu.check())
worasuchad 0:74030c9b6949 171 {
worasuchad 0:74030c9b6949 172 while (true)
worasuchad 0:74030c9b6949 173 {
worasuchad 0:74030c9b6949 174 led3 = !led3;
worasuchad 0:74030c9b6949 175 wait(0.1);
worasuchad 0:74030c9b6949 176 }
worasuchad 0:74030c9b6949 177 }
worasuchad 0:74030c9b6949 178
worasuchad 0:74030c9b6949 179 prev_control_time = timer1.read_ms();
worasuchad 0:74030c9b6949 180 while(1)
worasuchad 0:74030c9b6949 181 {
worasuchad 0:74030c9b6949 182 IMU(); // IMU func
worasuchad 0:74030c9b6949 183 if ((timer1.read_ms() - prev_control_time) >= 1) // read time in 1 ms
worasuchad 0:74030c9b6949 184 {
worasuchad 0:74030c9b6949 185 moveBase();
worasuchad 0:74030c9b6949 186 //mc.printf("%.3f\t\t", g_req_linear_vel_x);
worasuchad 0:74030c9b6949 187 //mc.printf("%.3f\t\t", g_req_angular_vel_z);
worasuchad 0:74030c9b6949 188 //mc.printf("%.3f\n\r", servo_steering_angle*2);
worasuchad 0:74030c9b6949 189
worasuchad 0:74030c9b6949 190 if(change_yaw < -5 && change_yaw > -300)
worasuchad 0:74030c9b6949 191 {
worasuchad 0:74030c9b6949 192 cYaw = 80;
worasuchad 0:74030c9b6949 193 }
worasuchad 0:74030c9b6949 194 else if(change_yaw < -300)
worasuchad 0:74030c9b6949 195 {
worasuchad 0:74030c9b6949 196 cYaw = -80;
worasuchad 0:74030c9b6949 197 }
worasuchad 0:74030c9b6949 198 else
worasuchad 0:74030c9b6949 199 {
worasuchad 0:74030c9b6949 200 cYaw = 0;
worasuchad 0:74030c9b6949 201 }
worasuchad 0:74030c9b6949 202
worasuchad 0:74030c9b6949 203 lamborSteer.write(servo_steering_angle*2 + 80 + cYaw);
worasuchad 0:74030c9b6949 204 forward = g_req_linear_vel_x/2;
worasuchad 0:74030c9b6949 205
worasuchad 0:74030c9b6949 206 prev_control_time = timer1.read_ms();
worasuchad 0:74030c9b6949 207 }
worasuchad 0:74030c9b6949 208 //IMU(); // IMU func
worasuchad 0:74030c9b6949 209 Ultrasonic(); // Ultrasonic func
worasuchad 0:74030c9b6949 210 nh.spinOnce();
worasuchad 0:74030c9b6949 211 wait_ms(1);
worasuchad 0:74030c9b6949 212 }
worasuchad 0:74030c9b6949 213 }
worasuchad 0:74030c9b6949 214
worasuchad 0:74030c9b6949 215 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 216 /* IMU */
worasuchad 0:74030c9b6949 217 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 218 void IMU()
worasuchad 0:74030c9b6949 219 {
worasuchad 0:74030c9b6949 220 imu.setmode(OPERATION_MODE_NDOF);
worasuchad 0:74030c9b6949 221 imu.get_calib();
worasuchad 0:74030c9b6949 222 imu.get_angles();
worasuchad 0:74030c9b6949 223 imu.get_temp();
worasuchad 0:74030c9b6949 224 imu.get_quat();
worasuchad 0:74030c9b6949 225
worasuchad 0:74030c9b6949 226 if(yawFirst)
worasuchad 0:74030c9b6949 227 {
worasuchad 0:74030c9b6949 228 init_yaw = imu.euler.yaw;
worasuchad 0:74030c9b6949 229 yawFirst = false;
worasuchad 0:74030c9b6949 230 }
worasuchad 0:74030c9b6949 231
worasuchad 0:74030c9b6949 232 yaw_angle = imu.euler.yaw;
worasuchad 0:74030c9b6949 233 change_yaw = init_yaw - yaw_angle;
worasuchad 0:74030c9b6949 234 yaw.data = yaw_angle;
worasuchad 0:74030c9b6949 235 pub_yaw.publish(&yaw);
worasuchad 0:74030c9b6949 236 //pc.printf("Temperature\t%d\n", imu.temperature);
worasuchad 0:74030c9b6949 237 //pc.printf("Quaternion: w:%f\tx:%f\ty:%f\tz:%f\n", imu.quat.w, imu.quat.x, imu.quat.y, imu.quat.z);
worasuchad 0:74030c9b6949 238 //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);
worasuchad 0:74030c9b6949 239 }
worasuchad 0:74030c9b6949 240
worasuchad 0:74030c9b6949 241 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 242 /* Ultrasonic */
worasuchad 0:74030c9b6949 243 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 244 void Ultrasonic()
worasuchad 0:74030c9b6949 245 {
worasuchad 0:74030c9b6949 246 mc.printf("sonic!");
worasuchad 0:74030c9b6949 247 voltL = pinL;
worasuchad 0:74030c9b6949 248 voltR = pinR;
worasuchad 0:74030c9b6949 249 cmL = voltL * 2000; //Takes bit count and converts it to mm
worasuchad 0:74030c9b6949 250 cmR = voltR * 2000;
worasuchad 0:74030c9b6949 251 //mc.printf("Distance L = %.3f cm\t\t", cmL);
worasuchad 0:74030c9b6949 252 //mc.printf("Distance R = %.3f cm\n\r", cmR);
worasuchad 0:74030c9b6949 253
worasuchad 0:74030c9b6949 254 if (cmL < 80 || cmR < 80 )
worasuchad 0:74030c9b6949 255 {
worasuchad 0:74030c9b6949 256 led1 = 1 ; // LED is ON
worasuchad 0:74030c9b6949 257 led2 = 1 ;
worasuchad 0:74030c9b6949 258 //led3 = 1 ;
worasuchad 0:74030c9b6949 259
worasuchad 0:74030c9b6949 260 forward = 0;
worasuchad 0:74030c9b6949 261 //leftMotor.brake();
worasuchad 0:74030c9b6949 262 //rightMotor.brake();
worasuchad 0:74030c9b6949 263 }
worasuchad 0:74030c9b6949 264 else
worasuchad 0:74030c9b6949 265 {
worasuchad 0:74030c9b6949 266 led1 = 0 ; // LED is OFF
worasuchad 0:74030c9b6949 267 led2 = 0 ;
worasuchad 0:74030c9b6949 268 //led3 = 0 ;
worasuchad 0:74030c9b6949 269 }
worasuchad 0:74030c9b6949 270 //wait(0.1);
worasuchad 0:74030c9b6949 271 }
worasuchad 0:74030c9b6949 272
worasuchad 0:74030c9b6949 273 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 274 /* checkST */
worasuchad 0:74030c9b6949 275 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 276
worasuchad 0:74030c9b6949 277 void watchSteer()
worasuchad 0:74030c9b6949 278 {
worasuchad 0:74030c9b6949 279 watchTimer = timer1.read_ms();
worasuchad 0:74030c9b6949 280 pubTimer = timer1.read_ms();
worasuchad 0:74030c9b6949 281 while(1)
worasuchad 0:74030c9b6949 282 {
worasuchad 0:74030c9b6949 283 //lamborSteer.timer_int_routine();
worasuchad 0:74030c9b6949 284 if ((timer1.read_ms() - watchTimer) >= 10) // read time in 10 ms
worasuchad 0:74030c9b6949 285 {
worasuchad 0:74030c9b6949 286 current_status = lamborSteer.read();
worasuchad 0:74030c9b6949 287 //mc.printf("%d\t\t", current_status.current_angle);
worasuchad 0:74030c9b6949 288 //mc.printf("%d\t\t", current_status.wanted_angle);
worasuchad 0:74030c9b6949 289 //mc.printf("%d\n\r", current_status.speed);
worasuchad 0:74030c9b6949 290 lamborSteer.timer_int_routine();
worasuchad 0:74030c9b6949 291
worasuchad 0:74030c9b6949 292 /*
worasuchad 0:74030c9b6949 293 if(current_status.current_angle < 200)
worasuchad 0:74030c9b6949 294 {
worasuchad 0:74030c9b6949 295 delta_st = -( (-PI * current_status.current_angle / 400) + (PI / 2) );
worasuchad 0:74030c9b6949 296 }
worasuchad 0:74030c9b6949 297 else if(current_status.current_angle >= 200)
worasuchad 0:74030c9b6949 298 {
worasuchad 0:74030c9b6949 299 delta_st = current_status.current_angle - 200;
worasuchad 0:74030c9b6949 300 delta_st = PI*delta_st / 400;
worasuchad 0:74030c9b6949 301 }
worasuchad 0:74030c9b6949 302
worasuchad 0:74030c9b6949 303 ackermann.data = delta_st;
worasuchad 0:74030c9b6949 304 Steer.publish( &ackermann );
worasuchad 0:74030c9b6949 305 */
worasuchad 0:74030c9b6949 306 watchTimer = timer1.read_ms();
worasuchad 0:74030c9b6949 307 }
worasuchad 0:74030c9b6949 308
worasuchad 0:74030c9b6949 309 /***********************/
worasuchad 0:74030c9b6949 310 if((timer1.read_ms() - pubTimer) >= 100) // read time in 100 ms
worasuchad 0:74030c9b6949 311 {
worasuchad 0:74030c9b6949 312 if(current_status.current_angle < 200)
worasuchad 0:74030c9b6949 313 {
worasuchad 0:74030c9b6949 314 delta_st = -( (-PI * current_status.current_angle / 400) + (PI / 2) );
worasuchad 0:74030c9b6949 315 }
worasuchad 0:74030c9b6949 316 else if(current_status.current_angle >= 200)
worasuchad 0:74030c9b6949 317 {
worasuchad 0:74030c9b6949 318 delta_st = current_status.current_angle - 200;
worasuchad 0:74030c9b6949 319 delta_st = PI*delta_st / 400;
worasuchad 0:74030c9b6949 320 }
worasuchad 0:74030c9b6949 321
worasuchad 0:74030c9b6949 322 ackermann.data = delta_st;
worasuchad 0:74030c9b6949 323 Steer.publish( &ackermann );
worasuchad 0:74030c9b6949 324
worasuchad 0:74030c9b6949 325 pubTimer = timer1.read_ms();
worasuchad 0:74030c9b6949 326 }
worasuchad 0:74030c9b6949 327 /***********************/
worasuchad 0:74030c9b6949 328 }
worasuchad 0:74030c9b6949 329 }
worasuchad 0:74030c9b6949 330
worasuchad 0:74030c9b6949 331 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 332 /* Drive */
worasuchad 0:74030c9b6949 333 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 334 void Drive()
worasuchad 0:74030c9b6949 335 {
worasuchad 0:74030c9b6949 336 while(1)
worasuchad 0:74030c9b6949 337 {
worasuchad 0:74030c9b6949 338 if(cmDrive) // Drive if cmDrive = true
worasuchad 0:74030c9b6949 339 {
worasuchad 0:74030c9b6949 340 leftPulses = leftQei.getPulses();
worasuchad 0:74030c9b6949 341 leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
worasuchad 0:74030c9b6949 342 leftPrevPulses = leftPulses;
worasuchad 0:74030c9b6949 343 leftPid.setProcessValue(fabs(leftVelocity));
worasuchad 0:74030c9b6949 344 leftMotor.speed(forward*leftPid.compute());
worasuchad 0:74030c9b6949 345
worasuchad 0:74030c9b6949 346 rightPulses = rightQei.getPulses();
worasuchad 0:74030c9b6949 347 rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
worasuchad 0:74030c9b6949 348 norTick = rightPulses - rightPrevPulses; // normarlize tick
worasuchad 0:74030c9b6949 349 rightPrevPulses = rightPulses;
worasuchad 0:74030c9b6949 350 rightPid.setProcessValue(fabs(rightVelocity));
worasuchad 0:74030c9b6949 351 rightMotor.speed(forward*rightPid.compute());
worasuchad 0:74030c9b6949 352
worasuchad 0:74030c9b6949 353 if( norTick >= 196) // 100 tick-per-r
worasuchad 0:74030c9b6949 354 {
worasuchad 0:74030c9b6949 355 numTick = numTick + 1;
worasuchad 0:74030c9b6949 356 Tick.data = numTick;
worasuchad 0:74030c9b6949 357 Encoder.publish( &Tick );
worasuchad 0:74030c9b6949 358 norTick = 0;
worasuchad 0:74030c9b6949 359 }
worasuchad 0:74030c9b6949 360 wait(0.01);
worasuchad 0:74030c9b6949 361 }
worasuchad 0:74030c9b6949 362 else
worasuchad 0:74030c9b6949 363 {
worasuchad 0:74030c9b6949 364 leftMotor.brake();
worasuchad 0:74030c9b6949 365 rightMotor.brake();
worasuchad 0:74030c9b6949 366 }
worasuchad 0:74030c9b6949 367 }
worasuchad 0:74030c9b6949 368 }
worasuchad 0:74030c9b6949 369
worasuchad 0:74030c9b6949 370 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 371 /* pressed */
worasuchad 0:74030c9b6949 372 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 373 void pressed()
worasuchad 0:74030c9b6949 374 {
worasuchad 0:74030c9b6949 375 lamborSteer.stop();
worasuchad 0:74030c9b6949 376 }
worasuchad 0:74030c9b6949 377
worasuchad 0:74030c9b6949 378 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 379 /* ros */
worasuchad 0:74030c9b6949 380 /*--------------------------------------------------------------------*/
worasuchad 0:74030c9b6949 381 void commandCallback(const geometry_msgs::Twist& cmd_msg)
worasuchad 0:74030c9b6949 382 {
worasuchad 0:74030c9b6949 383 //callback function every time linear and angular speed is received from 'cmd_vel' topic
worasuchad 0:74030c9b6949 384 //this callback function receives cmd_msg object where linear and angular speed are stored
worasuchad 0:74030c9b6949 385 g_req_linear_vel_x = cmd_msg.linear.x;
worasuchad 0:74030c9b6949 386 g_req_linear_vel_y = cmd_msg.linear.y;
worasuchad 0:74030c9b6949 387 g_req_angular_vel_z = cmd_msg.angular.z;
worasuchad 0:74030c9b6949 388
worasuchad 0:74030c9b6949 389 //pc.printf("%.3f\t\t", g_req_linear_vel_x);
worasuchad 0:74030c9b6949 390 //pc.printf("%.3f\t\t", g_req_linear_vel_y);
worasuchad 0:74030c9b6949 391 //pc.printf("%.3f\n\r", g_req_angular_vel_z);
worasuchad 0:74030c9b6949 392 g_prev_command_time = timer1.read_ms();;
worasuchad 0:74030c9b6949 393 }
worasuchad 0:74030c9b6949 394
worasuchad 0:74030c9b6949 395 void moveBase()
worasuchad 0:74030c9b6949 396 {
worasuchad 0:74030c9b6949 397 float current_steering_angle;
worasuchad 0:74030c9b6949 398 current_steering_angle = steer(g_req_angular_vel_z);
worasuchad 0:74030c9b6949 399 //pc.printf("%.3f\t\t", g_req_linear_vel_x);
worasuchad 0:74030c9b6949 400 //pc.printf("%.3f\t\t", g_req_linear_vel_y);
worasuchad 0:74030c9b6949 401 //pc.printf("%.3f\t\t", g_req_angular_vel_z);
worasuchad 0:74030c9b6949 402 //pc.printf("%.3f\n\r", current_steering_angle);
worasuchad 0:74030c9b6949 403 }
worasuchad 0:74030c9b6949 404
worasuchad 0:74030c9b6949 405 float steer(float steering_angle)
worasuchad 0:74030c9b6949 406 {
worasuchad 0:74030c9b6949 407 //steering function for ACKERMANN base
worasuchad 0:74030c9b6949 408 //float servo_steering_angle;
worasuchad 0:74030c9b6949 409
worasuchad 0:74030c9b6949 410 //steering_angle = constrain(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE);
worasuchad 0:74030c9b6949 411 servo_steering_angle = mapFloat(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE, 0, PI) * (180 / PI);
worasuchad 0:74030c9b6949 412
worasuchad 0:74030c9b6949 413 if(servo_steering_angle < 0)
worasuchad 0:74030c9b6949 414 {
worasuchad 0:74030c9b6949 415 servo_steering_angle = 0;
worasuchad 0:74030c9b6949 416 }
worasuchad 0:74030c9b6949 417 else if(servo_steering_angle > 200)
worasuchad 0:74030c9b6949 418 {
worasuchad 0:74030c9b6949 419 servo_steering_angle = 200;
worasuchad 0:74030c9b6949 420 }
worasuchad 0:74030c9b6949 421
worasuchad 0:74030c9b6949 422 //servo_steering_angle = mapFloat(steering_angle, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE, PI, 0) * (180 / PI);
worasuchad 0:74030c9b6949 423
worasuchad 0:74030c9b6949 424 //steering_servo.write(servo_steering_angle);
worasuchad 0:74030c9b6949 425
worasuchad 0:74030c9b6949 426 //return steering_angle;
worasuchad 0:74030c9b6949 427 return servo_steering_angle;
worasuchad 0:74030c9b6949 428 }
worasuchad 0:74030c9b6949 429
worasuchad 0:74030c9b6949 430 float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
worasuchad 0:74030c9b6949 431 {
worasuchad 0:74030c9b6949 432 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
worasuchad 0:74030c9b6949 433 }
worasuchad 0:74030c9b6949 434
worasuchad 0:74030c9b6949 435 /*--------------------------------------------------------------------*/