Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

orion_main.cpp@26:32eaf3c3ac2e, 2018-03-15 (annotated)

Committer:
ahmed_lv
Date:
Thu Mar 15 09:14:43 2018 +0000
Revision:
26:32eaf3c3ac2e
Parent:
25:3a82f868c101
Child:
27:2db168d9fb18
1st commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrynguyen 21:7cd86bea7f83 1 /**
harrynguyen 21:7cd86bea7f83 2 * @author Akash Vibhute
harrynguyen 21:7cd86bea7f83 3 * < akash . roboticist [at] gmail . com >
harrynguyen 21:7cd86bea7f83 4 *
harrynguyen 21:7cd86bea7f83 5 * @section LICENSE
harrynguyen 21:7cd86bea7f83 6 *
harrynguyen 21:7cd86bea7f83 7 * Copyright (c) 2015 Akash Vibhute
harrynguyen 21:7cd86bea7f83 8 *
harrynguyen 21:7cd86bea7f83 9 * Licensed under the Apache License, Version 2.0 (the "License");
harrynguyen 21:7cd86bea7f83 10 * you may not use this file except in compliance with the License.
harrynguyen 21:7cd86bea7f83 11 * You may obtain a copy of the License at
harrynguyen 21:7cd86bea7f83 12 *
harrynguyen 21:7cd86bea7f83 13 * http://www.apache.org/licenses/LICENSE-2.0
harrynguyen 21:7cd86bea7f83 14 *
harrynguyen 21:7cd86bea7f83 15 * Unless required by applicable law or agreed to in writing, software
harrynguyen 21:7cd86bea7f83 16 * distributed under the License is distributed on an "AS IS" BASIS,
harrynguyen 21:7cd86bea7f83 17 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
harrynguyen 21:7cd86bea7f83 18 * See the License for the specific language governing permissions and
harrynguyen 21:7cd86bea7f83 19 * limitations under the License.
harrynguyen 21:7cd86bea7f83 20 *
harrynguyen 21:7cd86bea7f83 21 * The above copyright notice and this permission notice shall be included in
harrynguyen 21:7cd86bea7f83 22 * all copies or substantial portions of the Software.
harrynguyen 21:7cd86bea7f83 23 *
harrynguyen 21:7cd86bea7f83 24 * @section DESCRIPTION
harrynguyen 21:7cd86bea7f83 25 *
harrynguyen 21:7cd86bea7f83 26 * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
harrynguyen 21:7cd86bea7f83 27 * Robot controller software for SUTD Virgo version 3 robot
harrynguyen 21:7cd86bea7f83 28 *
harrynguyen 21:7cd86bea7f83 29 */
harrynguyen 21:7cd86bea7f83 30
harrynguyen 21:7cd86bea7f83 31 /**
harrynguyen 21:7cd86bea7f83 32 * Header file for including all necessary functions for robot and defining any
harrynguyen 21:7cd86bea7f83 33 * custom functions written in the main file.
harrynguyen 21:7cd86bea7f83 34 */
harrynguyen 21:7cd86bea7f83 35 #include "main.h"
harrynguyen 21:7cd86bea7f83 36 #include "globalExterns.h"
harrynguyen 21:7cd86bea7f83 37
harrynguyen 21:7cd86bea7f83 38 /**
harrynguyen 21:7cd86bea7f83 39 * Functions, Threads & General Definitions
harrynguyen 21:7cd86bea7f83 40 */
harrynguyen 21:7cd86bea7f83 41 //*****************************************************************************
harrynguyen 21:7cd86bea7f83 42 //** Drivetrain **
harrynguyen 21:7cd86bea7f83 43 motorDriver drive; //motor drive train
harrynguyen 21:7cd86bea7f83 44 odometer odometry; //odometer function
ahmed_lv 26:32eaf3c3ac2e 45 pidBearing PID_B; //PID control for bearing
harrynguyen 21:7cd86bea7f83 46 pidControl PID_L, PID_R; //pidcontroller for left and right motors
harrynguyen 21:7cd86bea7f83 47
ahmed_lv 26:32eaf3c3ac2e 48 //**new PID control**
ahmed_lv 26:32eaf3c3ac2e 49
ahmed_lv 26:32eaf3c3ac2e 50 float bearingToWaypoint;
ahmed_lv 26:32eaf3c3ac2e 51 float normalDistanceError;
ahmed_lv 26:32eaf3c3ac2e 52 float prevError;
ahmed_lv 26:32eaf3c3ac2e 53 float speedChange;
ahmed_lv 26:32eaf3c3ac2e 54
ahmed_lv 26:32eaf3c3ac2e 55 float k3, cmdL, cmdR, aError;
ahmed_lv 26:32eaf3c3ac2e 56 float dist[3];
ahmed_lv 26:32eaf3c3ac2e 57
ahmed_lv 26:32eaf3c3ac2e 58
ahmed_lv 26:32eaf3c3ac2e 59 //**end of new control **
ahmed_lv 26:32eaf3c3ac2e 60
harrynguyen 21:7cd86bea7f83 61 Timer motorControl_t;
harrynguyen 21:7cd86bea7f83 62 float rpm_cmd[2]; //drive motor rpm command
harrynguyen 21:7cd86bea7f83 63 float rpm_compensated[2]; //rpm command compensated by acc limit
harrynguyen 21:7cd86bea7f83 64 float targetAcceleration = 300.0; //RPM/s acceleration
harrynguyen 21:7cd86bea7f83 65 float pwm_cmd[2]; //drive motor pwm command
harrynguyen 21:7cd86bea7f83 66
harrynguyen 21:7cd86bea7f83 67 /* THREAD */
harrynguyen 21:7cd86bea7f83 68 void odometry_thread(void const *n);
harrynguyen 21:7cd86bea7f83 69 void motorControl_thread(void const *n);
harrynguyen 21:7cd86bea7f83 70 /* ** */
harrynguyen 21:7cd86bea7f83 71 //-------------
harrynguyen 21:7cd86bea7f83 72
harrynguyen 21:7cd86bea7f83 73 //** Localization **
harrynguyen 21:7cd86bea7f83 74 IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class
harrynguyen 21:7cd86bea7f83 75 float imuTime;
harrynguyen 21:7cd86bea7f83 76 Localization localization; //localization function
harrynguyen 21:7cd86bea7f83 77
harrynguyen 21:7cd86bea7f83 78 /* FUNCTION */
harrynguyen 21:7cd86bea7f83 79 bool imuInit_function();
harrynguyen 21:7cd86bea7f83 80 /* ** */
harrynguyen 21:7cd86bea7f83 81
harrynguyen 21:7cd86bea7f83 82 /* THREAD */
harrynguyen 21:7cd86bea7f83 83 void imu_thread(void const *n);
harrynguyen 21:7cd86bea7f83 84 /* ** */
harrynguyen 21:7cd86bea7f83 85 //-------------
harrynguyen 21:7cd86bea7f83 86
harrynguyen 21:7cd86bea7f83 87 //** Communications **
harrynguyen 23:6806c3bacf58 88 //nRF24NetworkHandler comm; //nRF24 radio and network layer handler function
harrynguyen 23:6806c3bacf58 89 //uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
harrynguyen 23:6806c3bacf58 90 //uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status,
harrynguyen 21:7cd86bea7f83 91
harrynguyen 23:6806c3bacf58 92 //cmdParser wirelessCmd;
harrynguyen 21:7cd86bea7f83 93
harrynguyen 23:6806c3bacf58 94
harrynguyen 21:7cd86bea7f83 95 /* ** */
harrynguyen 21:7cd86bea7f83 96 //-------------
harrynguyen 21:7cd86bea7f83 97
harrynguyen 21:7cd86bea7f83 98 //** Power Monitor **
harrynguyen 21:7cd86bea7f83 99 BattGuage battery; //Battery fuel gauge wrapper
harrynguyen 21:7cd86bea7f83 100
harrynguyen 21:7cd86bea7f83 101 /* THREAD */
harrynguyen 21:7cd86bea7f83 102
harrynguyen 21:7cd86bea7f83 103 /* ** */
harrynguyen 21:7cd86bea7f83 104 //-------------
harrynguyen 21:7cd86bea7f83 105
harrynguyen 21:7cd86bea7f83 106 //** Trajectory tracking **
harrynguyen 21:7cd86bea7f83 107 purePursuit purePursuit;
harrynguyen 23:6806c3bacf58 108 //kinematics kinematics;
harrynguyen 21:7cd86bea7f83 109
harrynguyen 21:7cd86bea7f83 110 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
harrynguyen 21:7cd86bea7f83 111 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
harrynguyen 21:7cd86bea7f83 112 uint8_t totalWaypoints = 5;
harrynguyen 21:7cd86bea7f83 113 //kite pattern 200cm long, 100cm wide
harrynguyen 21:7cd86bea7f83 114 int16_t waypoints_set[][4] = { {0,0,90,0},
harrynguyen 21:7cd86bea7f83 115 {100,100,90,0},
harrynguyen 21:7cd86bea7f83 116 {0,200,90,0},
harrynguyen 21:7cd86bea7f83 117 {-100,100,90,0},
harrynguyen 21:7cd86bea7f83 118 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 119 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 120 {0,0,90,0}
harrynguyen 21:7cd86bea7f83 121 };
harrynguyen 21:7cd86bea7f83 122
harrynguyen 21:7cd86bea7f83 123 float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
harrynguyen 21:7cd86bea7f83 124 uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
harrynguyen 21:7cd86bea7f83 125 uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
harrynguyen 21:7cd86bea7f83 126 float target_waypoint[2] = {0.0, 0.0}; //coordinates in millimeters for pure-pursuit's use. initialize with 0,0 this is necessary to prevent comparison to a garbage value
harrynguyen 21:7cd86bea7f83 127 float target_velocity =0.0; //target velocity in mm/s
harrynguyen 21:7cd86bea7f83 128 float distanceToWaypoint; //distance from robot to waypoint
harrynguyen 21:7cd86bea7f83 129 uint8_t waypoint_index=0;
harrynguyen 21:7cd86bea7f83 130 uint8_t go_cmd=0; //make robot run a waypoint set
harrynguyen 21:7cd86bea7f83 131
harrynguyen 21:7cd86bea7f83 132 /* THREAD */
harrynguyen 21:7cd86bea7f83 133 void purePursuit_thread(void const *n);
harrynguyen 21:7cd86bea7f83 134 void waypointCmd_thread(void const *n);
harrynguyen 21:7cd86bea7f83 135 /* ** */
harrynguyen 21:7cd86bea7f83 136 //-------------
harrynguyen 21:7cd86bea7f83 137
harrynguyen 21:7cd86bea7f83 138 //** Attitude control **
harrynguyen 21:7cd86bea7f83 139 attitudeControl attitudeControl;
harrynguyen 23:6806c3bacf58 140 //pidAttitudeControl pidPitchControl;
harrynguyen 23:6806c3bacf58 141 //pidAttitudeControl pidRollControl;
harrynguyen 21:7cd86bea7f83 142
harrynguyen 21:7cd86bea7f83 143 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
harrynguyen 21:7cd86bea7f83 144
harrynguyen 21:7cd86bea7f83 145 /* THREAD */
harrynguyen 21:7cd86bea7f83 146
harrynguyen 21:7cd86bea7f83 147 /* ** */
harrynguyen 21:7cd86bea7f83 148 //-------------
harrynguyen 21:7cd86bea7f83 149
harrynguyen 21:7cd86bea7f83 150
harrynguyen 21:7cd86bea7f83 151 /* ** */
harrynguyen 21:7cd86bea7f83 152 //-------------
harrynguyen 21:7cd86bea7f83 153
harrynguyen 23:6806c3bacf58 154 ////** Robot data recorder **
harrynguyen 23:6806c3bacf58 155 //RDR virgoRDR;
harrynguyen 23:6806c3bacf58 156 //uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled
harrynguyen 21:7cd86bea7f83 157
harrynguyen 21:7cd86bea7f83 158 /* THREAD */
harrynguyen 21:7cd86bea7f83 159
harrynguyen 21:7cd86bea7f83 160 /* ** */
harrynguyen 21:7cd86bea7f83 161 //-------------
harrynguyen 21:7cd86bea7f83 162
harrynguyen 21:7cd86bea7f83 163 //** Declarations of misc functions **
harrynguyen 21:7cd86bea7f83 164 Serial Debug(uart_TX, uart_RX); //Debug serial port
harrynguyen 21:7cd86bea7f83 165 DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication
harrynguyen 21:7cd86bea7f83 166
harrynguyen 21:7cd86bea7f83 167 /* THREAD */
harrynguyen 21:7cd86bea7f83 168 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 169 void print_thread(void const *n); //Debug printing thread
harrynguyen 21:7cd86bea7f83 170 /* ** */
harrynguyen 21:7cd86bea7f83 171 //-------------
harrynguyen 21:7cd86bea7f83 172 //-----------------------------------------------------------------------------
harrynguyen 21:7cd86bea7f83 173
harrynguyen 21:7cd86bea7f83 174
harrynguyen 21:7cd86bea7f83 175 int main()
harrynguyen 21:7cd86bea7f83 176 {
harrynguyen 21:7cd86bea7f83 177 Debug.baud(PC_BAUDRATE);
harrynguyen 21:7cd86bea7f83 178
harrynguyen 21:7cd86bea7f83 179 debugLED =1;
harrynguyen 21:7cd86bea7f83 180
harrynguyen 21:7cd86bea7f83 181 //wait_ms(5000);
harrynguyen 21:7cd86bea7f83 182
harrynguyen 21:7cd86bea7f83 183 Debug.printf("** Starting Virgo v3 Routines *************\n\n");
harrynguyen 21:7cd86bea7f83 184
harrynguyen 21:7cd86bea7f83 185 //** start Hearbeat loop as a thread **
harrynguyen 21:7cd86bea7f83 186 Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 187 Debug.printf("* Hearbeat loop started *\n");
harrynguyen 21:7cd86bea7f83 188
harrynguyen 21:7cd86bea7f83 189 //** start IMU funtion as Thread **
harrynguyen 21:7cd86bea7f83 190 Thread IMU_function(imu_thread, NULL, osPriorityHigh);
harrynguyen 21:7cd86bea7f83 191 Debug.printf("* IMU routine started *\n");
harrynguyen 21:7cd86bea7f83 192
harrynguyen 21:7cd86bea7f83 193 //** start OdometryUpdate function as Thread **
harrynguyen 21:7cd86bea7f83 194 Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 195 Debug.printf("* Odometry routine started *\n");
harrynguyen 21:7cd86bea7f83 196
harrynguyen 21:7cd86bea7f83 197 //** start MotorControl function as Thread **
harrynguyen 21:7cd86bea7f83 198 Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 199 Debug.printf("* Motor control routine started *\n");
harrynguyen 21:7cd86bea7f83 200
harrynguyen 21:7cd86bea7f83 201 //** start PurePursuit controller as Thread **
harrynguyen 21:7cd86bea7f83 202 Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 203 Debug.printf("* PurePursuit controller routine started *\n");
harrynguyen 21:7cd86bea7f83 204
harrynguyen 21:7cd86bea7f83 205 //** start Waypoint commander function as Thread **
harrynguyen 21:7cd86bea7f83 206 Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 207 Debug.printf("* Waypoint commander routine started *\n");
harrynguyen 21:7cd86bea7f83 208
harrynguyen 21:7cd86bea7f83 209 //** start Debug print loop as a thread **
harrynguyen 21:7cd86bea7f83 210 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 211 Debug.printf("* Print loop started *\n\n\n");
harrynguyen 21:7cd86bea7f83 212
harrynguyen 21:7cd86bea7f83 213 Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
harrynguyen 21:7cd86bea7f83 214
harrynguyen 21:7cd86bea7f83 215 while(1) {
harrynguyen 21:7cd86bea7f83 216
harrynguyen 21:7cd86bea7f83 217 }
harrynguyen 21:7cd86bea7f83 218 }
harrynguyen 21:7cd86bea7f83 219
harrynguyen 21:7cd86bea7f83 220 /**
harrynguyen 21:7cd86bea7f83 221 * heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 222 */
harrynguyen 21:7cd86bea7f83 223 void heartbeat_thread(void const *n)
harrynguyen 21:7cd86bea7f83 224 {
harrynguyen 21:7cd86bea7f83 225 while(true) {
harrynguyen 21:7cd86bea7f83 226 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 227 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 228 Thread::wait(Hearbeat_RateMS-50);
harrynguyen 21:7cd86bea7f83 229 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 230 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 231 } else
harrynguyen 21:7cd86bea7f83 232 debugLED = 1;
harrynguyen 21:7cd86bea7f83 233 }
harrynguyen 21:7cd86bea7f83 234 }
harrynguyen 21:7cd86bea7f83 235
harrynguyen 21:7cd86bea7f83 236 /**
harrynguyen 21:7cd86bea7f83 237 * imu initialization function
harrynguyen 21:7cd86bea7f83 238 */
harrynguyen 21:7cd86bea7f83 239 bool imuInit_function()
harrynguyen 21:7cd86bea7f83 240 {
harrynguyen 21:7cd86bea7f83 241 return (imu.imuInit());
harrynguyen 21:7cd86bea7f83 242 }
harrynguyen 21:7cd86bea7f83 243
harrynguyen 21:7cd86bea7f83 244 /**
harrynguyen 21:7cd86bea7f83 245 * imu update loop as an individual thread
harrynguyen 21:7cd86bea7f83 246 */
harrynguyen 21:7cd86bea7f83 247 void imu_thread(void const *n)
harrynguyen 21:7cd86bea7f83 248 {
harrynguyen 21:7cd86bea7f83 249 bool init_status = imuInit_function();
harrynguyen 21:7cd86bea7f83 250 Thread::wait(100);
harrynguyen 21:7cd86bea7f83 251
harrynguyen 21:7cd86bea7f83 252 while(init_status) {
harrynguyen 21:7cd86bea7f83 253 imu.imuUpdate();
harrynguyen 21:7cd86bea7f83 254
harrynguyen 21:7cd86bea7f83 255 //Usage:
harrynguyen 21:7cd86bea7f83 256 //imu.Pose[0, 1, 2]; //euler x, y, z
harrynguyen 21:7cd86bea7f83 257 //imu.AngVel[0, 1, 2]; //AngVel x, y, z
harrynguyen 21:7cd86bea7f83 258 //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
harrynguyen 21:7cd86bea7f83 259 //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
harrynguyen 21:7cd86bea7f83 260
harrynguyen 21:7cd86bea7f83 261 imuTime = imu.time_s;
harrynguyen 21:7cd86bea7f83 262
harrynguyen 21:7cd86bea7f83 263 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 264 }
harrynguyen 21:7cd86bea7f83 265 }
harrynguyen 21:7cd86bea7f83 266
harrynguyen 21:7cd86bea7f83 267 /**
harrynguyen 21:7cd86bea7f83 268 * odometry update loop as an individual thread
harrynguyen 21:7cd86bea7f83 269 */
harrynguyen 21:7cd86bea7f83 270 void odometry_thread(void const *n)
harrynguyen 21:7cd86bea7f83 271 {
harrynguyen 21:7cd86bea7f83 272 odometry.init();
harrynguyen 21:7cd86bea7f83 273 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 274
harrynguyen 21:7cd86bea7f83 275 while(true) {
harrynguyen 21:7cd86bea7f83 276 odometry.update();
harrynguyen 21:7cd86bea7f83 277
harrynguyen 21:7cd86bea7f83 278 //Usage:
harrynguyen 21:7cd86bea7f83 279 //odometer.revolutions[0, 1]; //revolutions left, right
harrynguyen 21:7cd86bea7f83 280 //odometer.rpm[0, 1]; //rpm left, right
harrynguyen 21:7cd86bea7f83 281
harrynguyen 21:7cd86bea7f83 282 localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);
harrynguyen 21:7cd86bea7f83 283
harrynguyen 21:7cd86bea7f83 284 //Usage:
harrynguyen 21:7cd86bea7f83 285 //localization.position[0, 1] //x, y
harrynguyen 21:7cd86bea7f83 286
harrynguyen 21:7cd86bea7f83 287 Thread::wait(odometry_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 288 }
harrynguyen 21:7cd86bea7f83 289 }
harrynguyen 21:7cd86bea7f83 290
harrynguyen 21:7cd86bea7f83 291 /**/
harrynguyen 21:7cd86bea7f83 292 float rpm_smc = 500;
harrynguyen 21:7cd86bea7f83 293 float ref_dtheta = 0;
harrynguyen 21:7cd86bea7f83 294 float ref_theta = 0;
harrynguyen 21:7cd86bea7f83 295
harrynguyen 21:7cd86bea7f83 296 float ref_dgamma = 0;
harrynguyen 21:7cd86bea7f83 297 float ref_gamma = 0;
harrynguyen 21:7cd86bea7f83 298
harrynguyen 21:7cd86bea7f83 299 float ref_beta = DEG_TO_RAD(0.0);
harrynguyen 21:7cd86bea7f83 300 float ref_dbeta = 0;
harrynguyen 21:7cd86bea7f83 301
harrynguyen 21:7cd86bea7f83 302 float u1, u2;
harrynguyen 21:7cd86bea7f83 303 /**/
harrynguyen 21:7cd86bea7f83 304 /**
harrynguyen 21:7cd86bea7f83 305 * motor control loop as an individual thread
harrynguyen 21:7cd86bea7f83 306 */
harrynguyen 21:7cd86bea7f83 307 void motorControl_thread(void const *n)
harrynguyen 21:7cd86bea7f83 308 {
harrynguyen 21:7cd86bea7f83 309 motorControl_t.start();
harrynguyen 21:7cd86bea7f83 310
harrynguyen 21:7cd86bea7f83 311 float pitch_th, pitch_om, yaw_th, yaw_om;
harrynguyen 21:7cd86bea7f83 312 float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
harrynguyen 21:7cd86bea7f83 313 float W_l, W_r;
harrynguyen 21:7cd86bea7f83 314
harrynguyen 21:7cd86bea7f83 315 while(true) {
harrynguyen 21:7cd86bea7f83 316
harrynguyen 21:7cd86bea7f83 317 //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
harrynguyen 21:7cd86bea7f83 318 if(imu.imu_stabilized[1] ==1) {
harrynguyen 21:7cd86bea7f83 319
harrynguyen 21:7cd86bea7f83 320 pitch_th = DEG_TO_RAD(imu.Pose[0]);
harrynguyen 21:7cd86bea7f83 321 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 21:7cd86bea7f83 322 if(pitch_th > M_PI) pitch_th -= 2*M_PI;
harrynguyen 21:7cd86bea7f83 323 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 21:7cd86bea7f83 324
harrynguyen 21:7cd86bea7f83 325
harrynguyen 21:7cd86bea7f83 326 pitch_om = DEG_TO_RAD(imu.AngVel[0]);
harrynguyen 21:7cd86bea7f83 327
harrynguyen 21:7cd86bea7f83 328 yaw_th = DEG_TO_RAD(imu.Pose[2]);
harrynguyen 21:7cd86bea7f83 329
harrynguyen 21:7cd86bea7f83 330 yaw_om = DEG_TO_RAD(imu.AngVel[2]);
harrynguyen 21:7cd86bea7f83 331
harrynguyen 21:7cd86bea7f83 332 wheelTh_l = odometry.revolutions[0]*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 333 wheelTh_r = odometry.revolutions[1]*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 334
harrynguyen 21:7cd86bea7f83 335 wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 336 wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 337
harrynguyen 21:7cd86bea7f83 338 //ref_dtheta = rpm_smc*(-2*M_PI/60.0); //*generalFunctions::abs_f(sin(2*M_PI*imuTime/(15*2)));
harrynguyen 21:7cd86bea7f83 339 //ref_theta = ref_dtheta * imuTime;
harrynguyen 21:7cd86bea7f83 340 //ref_gamma = DEG_TO_RAD(30.0) ;//* sin(2*M_PI*imuTime/(30*2));
harrynguyen 21:7cd86bea7f83 341
harrynguyen 21:7cd86bea7f83 342 ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
harrynguyen 21:7cd86bea7f83 343 ref_gamma = purePursuit_gamma;
harrynguyen 21:7cd86bea7f83 344 ref_dgamma = purePursuit_omega;
harrynguyen 21:7cd86bea7f83 345
ahmed_lv 26:32eaf3c3ac2e 346 // attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
ahmed_lv 26:32eaf3c3ac2e 347 // pitch_th, pitch_om, yaw_th , yaw_om,
ahmed_lv 26:32eaf3c3ac2e 348 // wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
ahmed_lv 26:32eaf3c3ac2e 349 // 0, ref_dbeta, ref_beta,
ahmed_lv 26:32eaf3c3ac2e 350 // 0, ref_dtheta, ref_theta,
ahmed_lv 26:32eaf3c3ac2e 351 // 0, ref_dgamma, ref_gamma,
ahmed_lv 26:32eaf3c3ac2e 352 // &u1, &u2);
ahmed_lv 26:32eaf3c3ac2e 353
ahmed_lv 26:32eaf3c3ac2e 354 PID_B.setSpeedChange( &W_l, &W_r, &speedChange, &aError, target_waypoint[0], target_waypoint[1],
ahmed_lv 26:32eaf3c3ac2e 355 localization.position[0], localization.position[1], imu.Pose[2], purePursuit.robotFrame_targetDistance,
ahmed_lv 26:32eaf3c3ac2e 356 waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3 ); //new controller
harrynguyen 21:7cd86bea7f83 357
harrynguyen 21:7cd86bea7f83 358
harrynguyen 21:7cd86bea7f83 359
harrynguyen 21:7cd86bea7f83 360 if(waypointSetFinish_flag == 0) {
harrynguyen 25:3a82f868c101 361 rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
harrynguyen 25:3a82f868c101 362 rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
harrynguyen 25:3a82f868c101 363 // rpm_cmd[0]=-800;
harrynguyen 25:3a82f868c101 364 // rpm_cmd[1]=-800;
harrynguyen 21:7cd86bea7f83 365
harrynguyen 21:7cd86bea7f83 366 if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) )
harrynguyen 21:7cd86bea7f83 367 rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);
harrynguyen 21:7cd86bea7f83 368 else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0)
harrynguyen 21:7cd86bea7f83 369 rpm_cmd[0] = 0;
harrynguyen 21:7cd86bea7f83 370
harrynguyen 21:7cd86bea7f83 371 if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) )
harrynguyen 21:7cd86bea7f83 372 rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]);
harrynguyen 21:7cd86bea7f83 373 else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0)
harrynguyen 21:7cd86bea7f83 374 rpm_cmd[1] = 0;
harrynguyen 21:7cd86bea7f83 375
harrynguyen 21:7cd86bea7f83 376 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 377 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 378
harrynguyen 21:7cd86bea7f83 379 //rpm_compensated[0]= rpm_cmd[0];
harrynguyen 21:7cd86bea7f83 380 //rpm_compensated[1]= rpm_cmd[1];
harrynguyen 21:7cd86bea7f83 381
harrynguyen 21:7cd86bea7f83 382 } else {
harrynguyen 21:7cd86bea7f83 383 rpm_cmd[0]=0;
harrynguyen 21:7cd86bea7f83 384 rpm_cmd[1]=0;
harrynguyen 21:7cd86bea7f83 385
harrynguyen 21:7cd86bea7f83 386 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 387 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 388 }
harrynguyen 21:7cd86bea7f83 389
harrynguyen 21:7cd86bea7f83 390 pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read());
harrynguyen 21:7cd86bea7f83 391 pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read());
harrynguyen 21:7cd86bea7f83 392
harrynguyen 21:7cd86bea7f83 393 drive.setPWM_L(pwm_cmd[0]);
harrynguyen 21:7cd86bea7f83 394 drive.setPWM_R(pwm_cmd[1]);
ahmed_lv 26:32eaf3c3ac2e 395
harrynguyen 21:7cd86bea7f83 396
harrynguyen 21:7cd86bea7f83 397 }
harrynguyen 21:7cd86bea7f83 398
harrynguyen 21:7cd86bea7f83 399 motorControl_t.reset();
harrynguyen 21:7cd86bea7f83 400
harrynguyen 21:7cd86bea7f83 401 Thread::wait(motorControl_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 402 }
harrynguyen 21:7cd86bea7f83 403 }
harrynguyen 21:7cd86bea7f83 404
harrynguyen 21:7cd86bea7f83 405 /**
harrynguyen 21:7cd86bea7f83 406 * purepursuit loop as an individual thread
harrynguyen 21:7cd86bea7f83 407 */
harrynguyen 21:7cd86bea7f83 408 void purePursuit_thread(void const *n)
harrynguyen 21:7cd86bea7f83 409 {
harrynguyen 21:7cd86bea7f83 410 while(true) {
harrynguyen 21:7cd86bea7f83 411 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 412 //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 21:7cd86bea7f83 413 purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 21:7cd86bea7f83 414
harrynguyen 21:7cd86bea7f83 415 if(purePursuit.robotFrame_targetDistance <= waypointZone)
harrynguyen 21:7cd86bea7f83 416 waypointReached_flag = 1;
harrynguyen 21:7cd86bea7f83 417 else
harrynguyen 21:7cd86bea7f83 418 waypointReached_flag = 0;
harrynguyen 21:7cd86bea7f83 419 }
harrynguyen 21:7cd86bea7f83 420 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 421 }
harrynguyen 21:7cd86bea7f83 422 }
harrynguyen 21:7cd86bea7f83 423
harrynguyen 21:7cd86bea7f83 424 /**
harrynguyen 21:7cd86bea7f83 425 * waypoint tracking loop as individual thread
harrynguyen 21:7cd86bea7f83 426 */
harrynguyen 21:7cd86bea7f83 427 void waypointCmd_thread(void const *n)
harrynguyen 21:7cd86bea7f83 428 {
harrynguyen 21:7cd86bea7f83 429 while(true) {
harrynguyen 21:7cd86bea7f83 430 //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
harrynguyen 21:7cd86bea7f83 431 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 432 if(waypoint_index > totalWaypoints) {
harrynguyen 21:7cd86bea7f83 433 target_velocity = 0.0; //stop the robot
harrynguyen 21:7cd86bea7f83 434 waypointSetFinish_flag = 1;
harrynguyen 21:7cd86bea7f83 435 }
harrynguyen 21:7cd86bea7f83 436
harrynguyen 21:7cd86bea7f83 437 if(waypointReached_flag == 1 && waypointSetFinish_flag == 0) {
harrynguyen 21:7cd86bea7f83 438 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 21:7cd86bea7f83 439 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 21:7cd86bea7f83 440 target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
harrynguyen 21:7cd86bea7f83 441 waypoint_index++;
harrynguyen 21:7cd86bea7f83 442 }
harrynguyen 21:7cd86bea7f83 443 }
harrynguyen 21:7cd86bea7f83 444 Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
harrynguyen 21:7cd86bea7f83 445 }
harrynguyen 21:7cd86bea7f83 446 }
harrynguyen 21:7cd86bea7f83 447
harrynguyen 21:7cd86bea7f83 448
harrynguyen 21:7cd86bea7f83 449
harrynguyen 21:7cd86bea7f83 450
harrynguyen 21:7cd86bea7f83 451
harrynguyen 21:7cd86bea7f83 452
harrynguyen 21:7cd86bea7f83 453
harrynguyen 21:7cd86bea7f83 454 /**
harrynguyen 21:7cd86bea7f83 455 * Debug data print loop as an individual thread
harrynguyen 21:7cd86bea7f83 456 */
harrynguyen 21:7cd86bea7f83 457 #define print_lines 15 //number of info lines being printed on screen
harrynguyen 21:7cd86bea7f83 458 void print_thread(void const *n)
harrynguyen 21:7cd86bea7f83 459 {
harrynguyen 21:7cd86bea7f83 460 //clear 14 lines while going up, these are the initilization lines printed on screen
harrynguyen 21:7cd86bea7f83 461 for(int l=14; l>0; l--) {
harrynguyen 21:7cd86bea7f83 462 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 463 Debug.printf("\e[K"); //clear line
harrynguyen 21:7cd86bea7f83 464 }
harrynguyen 21:7cd86bea7f83 465
harrynguyen 21:7cd86bea7f83 466 Debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
harrynguyen 21:7cd86bea7f83 467 for(int l=print_lines; l>0; l--) Debug.printf("\n");
harrynguyen 21:7cd86bea7f83 468 Debug.printf("\n===================================================");
harrynguyen 21:7cd86bea7f83 469 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 470
harrynguyen 21:7cd86bea7f83 471 while(true) {
harrynguyen 21:7cd86bea7f83 472 //move cursor up # of lines printed to create a static display and clear the first line
harrynguyen 21:7cd86bea7f83 473 for(int l=print_lines; l>0; l--) Debug.printf("\e[1A");
harrynguyen 21:7cd86bea7f83 474 Debug.printf("\e[K");
harrynguyen 21:7cd86bea7f83 475
harrynguyen 21:7cd86bea7f83 476 Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
harrynguyen 21:7cd86bea7f83 477 Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
harrynguyen 21:7cd86bea7f83 478 Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
harrynguyen 21:7cd86bea7f83 479 Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
harrynguyen 21:7cd86bea7f83 480 Debug.printf("Calib Status (M-A-G-S-O): (%d , %d , %d , %d , %d)\n\e[K", imu.calib_stat[0], imu.calib_stat[1], imu.calib_stat[2], imu.calib_stat[3], imu.calib_stat[4]);
harrynguyen 21:7cd86bea7f83 481
harrynguyen 21:7cd86bea7f83 482 //Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
harrynguyen 21:7cd86bea7f83 483 Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
ahmed_lv 26:32eaf3c3ac2e 484 Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
ahmed_lv 26:32eaf3c3ac2e 485 Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", aError, speedChange, k3);
harrynguyen 21:7cd86bea7f83 486 Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
harrynguyen 21:7cd86bea7f83 487
ahmed_lv 26:32eaf3c3ac2e 488
harrynguyen 21:7cd86bea7f83 489
harrynguyen 21:7cd86bea7f83 490 Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
harrynguyen 21:7cd86bea7f83 491 //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
harrynguyen 21:7cd86bea7f83 492 Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
harrynguyen 21:7cd86bea7f83 493 //Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 21:7cd86bea7f83 494
harrynguyen 21:7cd86bea7f83 495 //Debug.printf("PID_L: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_L.PIDFf_terms[0], PID_L.PIDFf_terms[1], PID_L.PIDFf_terms[2], PID_L.PIDFf_terms[3], PID_L.Summ_term);
harrynguyen 21:7cd86bea7f83 496 //Debug.printf("PID_R: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_R.PIDFf_terms[0], PID_R.PIDFf_terms[1], PID_R.PIDFf_terms[2], PID_R.PIDFf_terms[3], PID_R.Summ_term);
harrynguyen 21:7cd86bea7f83 497
harrynguyen 23:6806c3bacf58 498
harrynguyen 21:7cd86bea7f83 499 //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
harrynguyen 21:7cd86bea7f83 500
harrynguyen 21:7cd86bea7f83 501 Thread::wait(PrintLoop_PeriodMS);
harrynguyen 21:7cd86bea7f83 502 }
harrynguyen 21:7cd86bea7f83 503 }