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@21:7cd86bea7f83, 2018-02-08 (annotated)

Committer:
harrynguyen
Date:
Thu Feb 08 02:13:47 2018 +0000
Revision:
21:7cd86bea7f83
Child:
23:6806c3bacf58
Publish

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
harrynguyen 21:7cd86bea7f83 45 pidControl PID_L, PID_R; //pidcontroller for left and right motors
harrynguyen 21:7cd86bea7f83 46
harrynguyen 21:7cd86bea7f83 47 Timer motorControl_t;
harrynguyen 21:7cd86bea7f83 48 float rpm_cmd[2]; //drive motor rpm command
harrynguyen 21:7cd86bea7f83 49 float rpm_compensated[2]; //rpm command compensated by acc limit
harrynguyen 21:7cd86bea7f83 50 float targetAcceleration = 300.0; //RPM/s acceleration
harrynguyen 21:7cd86bea7f83 51 float pwm_cmd[2]; //drive motor pwm command
harrynguyen 21:7cd86bea7f83 52
harrynguyen 21:7cd86bea7f83 53 /* THREAD */
harrynguyen 21:7cd86bea7f83 54 void odometry_thread(void const *n);
harrynguyen 21:7cd86bea7f83 55 void motorControl_thread(void const *n);
harrynguyen 21:7cd86bea7f83 56 /* ** */
harrynguyen 21:7cd86bea7f83 57 //-------------
harrynguyen 21:7cd86bea7f83 58
harrynguyen 21:7cd86bea7f83 59 //** Localization **
harrynguyen 21:7cd86bea7f83 60 IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class
harrynguyen 21:7cd86bea7f83 61 float imuTime;
harrynguyen 21:7cd86bea7f83 62 Localization localization; //localization function
harrynguyen 21:7cd86bea7f83 63
harrynguyen 21:7cd86bea7f83 64 /* FUNCTION */
harrynguyen 21:7cd86bea7f83 65 bool imuInit_function();
harrynguyen 21:7cd86bea7f83 66 /* ** */
harrynguyen 21:7cd86bea7f83 67
harrynguyen 21:7cd86bea7f83 68 /* THREAD */
harrynguyen 21:7cd86bea7f83 69 void imu_thread(void const *n);
harrynguyen 21:7cd86bea7f83 70 /* ** */
harrynguyen 21:7cd86bea7f83 71 //-------------
harrynguyen 21:7cd86bea7f83 72
harrynguyen 21:7cd86bea7f83 73 //** Communications **
harrynguyen 21:7cd86bea7f83 74 nRF24NetworkHandler comm; //nRF24 radio and network layer handler function
harrynguyen 21:7cd86bea7f83 75 uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
harrynguyen 21:7cd86bea7f83 76 uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status,
harrynguyen 21:7cd86bea7f83 77
harrynguyen 21:7cd86bea7f83 78 cmdParser wirelessCmd;
harrynguyen 21:7cd86bea7f83 79
harrynguyen 21:7cd86bea7f83 80 /* THREAD */
harrynguyen 21:7cd86bea7f83 81 void comm_thread(void const *n);
harrynguyen 21:7cd86bea7f83 82 /* ** */
harrynguyen 21:7cd86bea7f83 83 //-------------
harrynguyen 21:7cd86bea7f83 84
harrynguyen 21:7cd86bea7f83 85 //** Power Monitor **
harrynguyen 21:7cd86bea7f83 86 BattGuage battery; //Battery fuel gauge wrapper
harrynguyen 21:7cd86bea7f83 87
harrynguyen 21:7cd86bea7f83 88 /* THREAD */
harrynguyen 21:7cd86bea7f83 89
harrynguyen 21:7cd86bea7f83 90 /* ** */
harrynguyen 21:7cd86bea7f83 91 //-------------
harrynguyen 21:7cd86bea7f83 92
harrynguyen 21:7cd86bea7f83 93 //** Trajectory tracking **
harrynguyen 21:7cd86bea7f83 94 purePursuit purePursuit;
harrynguyen 21:7cd86bea7f83 95 kinematics kinematics;
harrynguyen 21:7cd86bea7f83 96
harrynguyen 21:7cd86bea7f83 97 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
harrynguyen 21:7cd86bea7f83 98 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
harrynguyen 21:7cd86bea7f83 99 uint8_t totalWaypoints = 5;
harrynguyen 21:7cd86bea7f83 100 //kite pattern 200cm long, 100cm wide
harrynguyen 21:7cd86bea7f83 101 int16_t waypoints_set[][4] = { {0,0,90,0},
harrynguyen 21:7cd86bea7f83 102 {100,100,90,0},
harrynguyen 21:7cd86bea7f83 103 {0,200,90,0},
harrynguyen 21:7cd86bea7f83 104 {-100,100,90,0},
harrynguyen 21:7cd86bea7f83 105 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 106 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 107 {0,0,90,0}
harrynguyen 21:7cd86bea7f83 108 };
harrynguyen 21:7cd86bea7f83 109
harrynguyen 21:7cd86bea7f83 110 float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
harrynguyen 21:7cd86bea7f83 111 uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
harrynguyen 21:7cd86bea7f83 112 uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
harrynguyen 21:7cd86bea7f83 113 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 114 float target_velocity =0.0; //target velocity in mm/s
harrynguyen 21:7cd86bea7f83 115 float distanceToWaypoint; //distance from robot to waypoint
harrynguyen 21:7cd86bea7f83 116 uint8_t waypoint_index=0;
harrynguyen 21:7cd86bea7f83 117 uint8_t go_cmd=0; //make robot run a waypoint set
harrynguyen 21:7cd86bea7f83 118
harrynguyen 21:7cd86bea7f83 119 /* THREAD */
harrynguyen 21:7cd86bea7f83 120 void purePursuit_thread(void const *n);
harrynguyen 21:7cd86bea7f83 121 void waypointCmd_thread(void const *n);
harrynguyen 21:7cd86bea7f83 122 /* ** */
harrynguyen 21:7cd86bea7f83 123 //-------------
harrynguyen 21:7cd86bea7f83 124
harrynguyen 21:7cd86bea7f83 125 //** Attitude control **
harrynguyen 21:7cd86bea7f83 126 attitudeControl attitudeControl;
harrynguyen 21:7cd86bea7f83 127 pidAttitudeControl pidPitchControl;
harrynguyen 21:7cd86bea7f83 128 pidAttitudeControl pidRollControl;
harrynguyen 21:7cd86bea7f83 129
harrynguyen 21:7cd86bea7f83 130 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
harrynguyen 21:7cd86bea7f83 131
harrynguyen 21:7cd86bea7f83 132 /* THREAD */
harrynguyen 21:7cd86bea7f83 133
harrynguyen 21:7cd86bea7f83 134 /* ** */
harrynguyen 21:7cd86bea7f83 135 //-------------
harrynguyen 21:7cd86bea7f83 136
harrynguyen 21:7cd86bea7f83 137 //** Camera **
harrynguyen 21:7cd86bea7f83 138 camera camera; //camera driver
harrynguyen 21:7cd86bea7f83 139 uint8_t cameraFlag; //flag to enable camera: 0 - off, 1 - frequency controlled, 2 - permanantly on
harrynguyen 21:7cd86bea7f83 140
harrynguyen 21:7cd86bea7f83 141 /* THREAD */
harrynguyen 21:7cd86bea7f83 142 void cameraControl_thread(void const *n);
harrynguyen 21:7cd86bea7f83 143 /* ** */
harrynguyen 21:7cd86bea7f83 144 //-------------
harrynguyen 21:7cd86bea7f83 145
harrynguyen 21:7cd86bea7f83 146 //** Robot data recorder **
harrynguyen 21:7cd86bea7f83 147 RDR virgoRDR;
harrynguyen 21:7cd86bea7f83 148 uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled
harrynguyen 21:7cd86bea7f83 149
harrynguyen 21:7cd86bea7f83 150 /* THREAD */
harrynguyen 21:7cd86bea7f83 151
harrynguyen 21:7cd86bea7f83 152 /* ** */
harrynguyen 21:7cd86bea7f83 153 //-------------
harrynguyen 21:7cd86bea7f83 154
harrynguyen 21:7cd86bea7f83 155 //** Declarations of misc functions **
harrynguyen 21:7cd86bea7f83 156 Serial Debug(uart_TX, uart_RX); //Debug serial port
harrynguyen 21:7cd86bea7f83 157 DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication
harrynguyen 21:7cd86bea7f83 158
harrynguyen 21:7cd86bea7f83 159 /* THREAD */
harrynguyen 21:7cd86bea7f83 160 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 161 void print_thread(void const *n); //Debug printing thread
harrynguyen 21:7cd86bea7f83 162 /* ** */
harrynguyen 21:7cd86bea7f83 163 //-------------
harrynguyen 21:7cd86bea7f83 164 //-----------------------------------------------------------------------------
harrynguyen 21:7cd86bea7f83 165
harrynguyen 21:7cd86bea7f83 166
harrynguyen 21:7cd86bea7f83 167 int main()
harrynguyen 21:7cd86bea7f83 168 {
harrynguyen 21:7cd86bea7f83 169 Debug.baud(PC_BAUDRATE);
harrynguyen 21:7cd86bea7f83 170
harrynguyen 21:7cd86bea7f83 171 debugLED =1;
harrynguyen 21:7cd86bea7f83 172
harrynguyen 21:7cd86bea7f83 173 //wait_ms(5000);
harrynguyen 21:7cd86bea7f83 174
harrynguyen 21:7cd86bea7f83 175 Debug.printf("** Starting Virgo v3 Routines *************\n\n");
harrynguyen 21:7cd86bea7f83 176
harrynguyen 21:7cd86bea7f83 177 //** start Hearbeat loop as a thread **
harrynguyen 21:7cd86bea7f83 178 Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 179 Debug.printf("* Hearbeat loop started *\n");
harrynguyen 21:7cd86bea7f83 180
harrynguyen 21:7cd86bea7f83 181 //** start IMU funtion as Thread **
harrynguyen 21:7cd86bea7f83 182 Thread IMU_function(imu_thread, NULL, osPriorityHigh);
harrynguyen 21:7cd86bea7f83 183 Debug.printf("* IMU routine started *\n");
harrynguyen 21:7cd86bea7f83 184
harrynguyen 21:7cd86bea7f83 185 //** start OdometryUpdate function as Thread **
harrynguyen 21:7cd86bea7f83 186 Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 187 Debug.printf("* Odometry routine started *\n");
harrynguyen 21:7cd86bea7f83 188
harrynguyen 21:7cd86bea7f83 189 //** start MotorControl function as Thread **
harrynguyen 21:7cd86bea7f83 190 Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 191 Debug.printf("* Motor control routine started *\n");
harrynguyen 21:7cd86bea7f83 192
harrynguyen 21:7cd86bea7f83 193 //** start PurePursuit controller as Thread **
harrynguyen 21:7cd86bea7f83 194 Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 195 Debug.printf("* PurePursuit controller routine started *\n");
harrynguyen 21:7cd86bea7f83 196
harrynguyen 21:7cd86bea7f83 197 //** start Waypoint commander function as Thread **
harrynguyen 21:7cd86bea7f83 198 Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 199 Debug.printf("* Waypoint commander routine started *\n");
harrynguyen 21:7cd86bea7f83 200
harrynguyen 21:7cd86bea7f83 201 //** start comm loop as a thread **
harrynguyen 21:7cd86bea7f83 202 Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 203 Debug.printf("* Communications loop started *\n");
harrynguyen 21:7cd86bea7f83 204
harrynguyen 21:7cd86bea7f83 205 //** start camera control loop as a thread **
harrynguyen 21:7cd86bea7f83 206 Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 207 Debug.printf("* Camera control loop started *\n");
harrynguyen 21:7cd86bea7f83 208
harrynguyen 21:7cd86bea7f83 209 //** Start data recorder as Thread **
harrynguyen 21:7cd86bea7f83 210 //Thread dataRecorder_function(dataRecorder_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 211 //Debug.printf("* Data Recorder routine started *\n");
harrynguyen 21:7cd86bea7f83 212
harrynguyen 21:7cd86bea7f83 213 //** start Debug print loop as a thread **
harrynguyen 21:7cd86bea7f83 214 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 215 Debug.printf("* Print loop started *\n\n\n");
harrynguyen 21:7cd86bea7f83 216
harrynguyen 21:7cd86bea7f83 217 Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
harrynguyen 21:7cd86bea7f83 218
harrynguyen 21:7cd86bea7f83 219 while(1) {
harrynguyen 21:7cd86bea7f83 220
harrynguyen 21:7cd86bea7f83 221 }
harrynguyen 21:7cd86bea7f83 222 }
harrynguyen 21:7cd86bea7f83 223
harrynguyen 21:7cd86bea7f83 224 /**
harrynguyen 21:7cd86bea7f83 225 * heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 226 */
harrynguyen 21:7cd86bea7f83 227 void heartbeat_thread(void const *n)
harrynguyen 21:7cd86bea7f83 228 {
harrynguyen 21:7cd86bea7f83 229 while(true) {
harrynguyen 21:7cd86bea7f83 230 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 231 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 232 Thread::wait(Hearbeat_RateMS-50);
harrynguyen 21:7cd86bea7f83 233 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 234 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 235 } else
harrynguyen 21:7cd86bea7f83 236 debugLED = 1;
harrynguyen 21:7cd86bea7f83 237 }
harrynguyen 21:7cd86bea7f83 238 }
harrynguyen 21:7cd86bea7f83 239
harrynguyen 21:7cd86bea7f83 240 /**
harrynguyen 21:7cd86bea7f83 241 * imu initialization function
harrynguyen 21:7cd86bea7f83 242 */
harrynguyen 21:7cd86bea7f83 243 bool imuInit_function()
harrynguyen 21:7cd86bea7f83 244 {
harrynguyen 21:7cd86bea7f83 245 return (imu.imuInit());
harrynguyen 21:7cd86bea7f83 246 }
harrynguyen 21:7cd86bea7f83 247
harrynguyen 21:7cd86bea7f83 248 /**
harrynguyen 21:7cd86bea7f83 249 * imu update loop as an individual thread
harrynguyen 21:7cd86bea7f83 250 */
harrynguyen 21:7cd86bea7f83 251 void imu_thread(void const *n)
harrynguyen 21:7cd86bea7f83 252 {
harrynguyen 21:7cd86bea7f83 253 bool init_status = imuInit_function();
harrynguyen 21:7cd86bea7f83 254 Thread::wait(100);
harrynguyen 21:7cd86bea7f83 255
harrynguyen 21:7cd86bea7f83 256 while(init_status) {
harrynguyen 21:7cd86bea7f83 257 imu.imuUpdate();
harrynguyen 21:7cd86bea7f83 258
harrynguyen 21:7cd86bea7f83 259 //Usage:
harrynguyen 21:7cd86bea7f83 260 //imu.Pose[0, 1, 2]; //euler x, y, z
harrynguyen 21:7cd86bea7f83 261 //imu.AngVel[0, 1, 2]; //AngVel x, y, z
harrynguyen 21:7cd86bea7f83 262 //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
harrynguyen 21:7cd86bea7f83 263 //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
harrynguyen 21:7cd86bea7f83 264
harrynguyen 21:7cd86bea7f83 265 imuTime = imu.time_s;
harrynguyen 21:7cd86bea7f83 266
harrynguyen 21:7cd86bea7f83 267 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 268 }
harrynguyen 21:7cd86bea7f83 269 }
harrynguyen 21:7cd86bea7f83 270
harrynguyen 21:7cd86bea7f83 271 /**
harrynguyen 21:7cd86bea7f83 272 * odometry update loop as an individual thread
harrynguyen 21:7cd86bea7f83 273 */
harrynguyen 21:7cd86bea7f83 274 void odometry_thread(void const *n)
harrynguyen 21:7cd86bea7f83 275 {
harrynguyen 21:7cd86bea7f83 276 odometry.init();
harrynguyen 21:7cd86bea7f83 277 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 278
harrynguyen 21:7cd86bea7f83 279 while(true) {
harrynguyen 21:7cd86bea7f83 280 odometry.update();
harrynguyen 21:7cd86bea7f83 281
harrynguyen 21:7cd86bea7f83 282 //Usage:
harrynguyen 21:7cd86bea7f83 283 //odometer.revolutions[0, 1]; //revolutions left, right
harrynguyen 21:7cd86bea7f83 284 //odometer.rpm[0, 1]; //rpm left, right
harrynguyen 21:7cd86bea7f83 285
harrynguyen 21:7cd86bea7f83 286 localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);
harrynguyen 21:7cd86bea7f83 287
harrynguyen 21:7cd86bea7f83 288 //Usage:
harrynguyen 21:7cd86bea7f83 289 //localization.position[0, 1] //x, y
harrynguyen 21:7cd86bea7f83 290
harrynguyen 21:7cd86bea7f83 291 Thread::wait(odometry_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 292 }
harrynguyen 21:7cd86bea7f83 293 }
harrynguyen 21:7cd86bea7f83 294
harrynguyen 21:7cd86bea7f83 295 /**/
harrynguyen 21:7cd86bea7f83 296 float rpm_smc = 500;
harrynguyen 21:7cd86bea7f83 297 float ref_dtheta = 0;
harrynguyen 21:7cd86bea7f83 298 float ref_theta = 0;
harrynguyen 21:7cd86bea7f83 299
harrynguyen 21:7cd86bea7f83 300 float ref_dgamma = 0;
harrynguyen 21:7cd86bea7f83 301 float ref_gamma = 0;
harrynguyen 21:7cd86bea7f83 302
harrynguyen 21:7cd86bea7f83 303 float ref_beta = DEG_TO_RAD(0.0);
harrynguyen 21:7cd86bea7f83 304 float ref_dbeta = 0;
harrynguyen 21:7cd86bea7f83 305
harrynguyen 21:7cd86bea7f83 306 float u1, u2;
harrynguyen 21:7cd86bea7f83 307 /**/
harrynguyen 21:7cd86bea7f83 308 /**
harrynguyen 21:7cd86bea7f83 309 * motor control loop as an individual thread
harrynguyen 21:7cd86bea7f83 310 */
harrynguyen 21:7cd86bea7f83 311 void motorControl_thread(void const *n)
harrynguyen 21:7cd86bea7f83 312 {
harrynguyen 21:7cd86bea7f83 313 motorControl_t.start();
harrynguyen 21:7cd86bea7f83 314
harrynguyen 21:7cd86bea7f83 315 float pitch_th, pitch_om, yaw_th, yaw_om;
harrynguyen 21:7cd86bea7f83 316 float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
harrynguyen 21:7cd86bea7f83 317 float W_l, W_r;
harrynguyen 21:7cd86bea7f83 318
harrynguyen 21:7cd86bea7f83 319 while(true) {
harrynguyen 21:7cd86bea7f83 320
harrynguyen 21:7cd86bea7f83 321 //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
harrynguyen 21:7cd86bea7f83 322 if(imu.imu_stabilized[1] ==1) {
harrynguyen 21:7cd86bea7f83 323
harrynguyen 21:7cd86bea7f83 324 pitch_th = DEG_TO_RAD(imu.Pose[0]);
harrynguyen 21:7cd86bea7f83 325 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 21:7cd86bea7f83 326 if(pitch_th > M_PI) pitch_th -= 2*M_PI;
harrynguyen 21:7cd86bea7f83 327 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 21:7cd86bea7f83 328
harrynguyen 21:7cd86bea7f83 329
harrynguyen 21:7cd86bea7f83 330 pitch_om = DEG_TO_RAD(imu.AngVel[0]);
harrynguyen 21:7cd86bea7f83 331
harrynguyen 21:7cd86bea7f83 332 yaw_th = DEG_TO_RAD(imu.Pose[2]);
harrynguyen 21:7cd86bea7f83 333
harrynguyen 21:7cd86bea7f83 334 yaw_om = DEG_TO_RAD(imu.AngVel[2]);
harrynguyen 21:7cd86bea7f83 335
harrynguyen 21:7cd86bea7f83 336 wheelTh_l = odometry.revolutions[0]*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 337 wheelTh_r = odometry.revolutions[1]*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 338
harrynguyen 21:7cd86bea7f83 339 wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 340 wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
harrynguyen 21:7cd86bea7f83 341
harrynguyen 21:7cd86bea7f83 342 //ref_dtheta = rpm_smc*(-2*M_PI/60.0); //*generalFunctions::abs_f(sin(2*M_PI*imuTime/(15*2)));
harrynguyen 21:7cd86bea7f83 343 //ref_theta = ref_dtheta * imuTime;
harrynguyen 21:7cd86bea7f83 344 //ref_gamma = DEG_TO_RAD(30.0) ;//* sin(2*M_PI*imuTime/(30*2));
harrynguyen 21:7cd86bea7f83 345
harrynguyen 21:7cd86bea7f83 346 ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
harrynguyen 21:7cd86bea7f83 347 ref_gamma = purePursuit_gamma;
harrynguyen 21:7cd86bea7f83 348 ref_dgamma = purePursuit_omega;
harrynguyen 21:7cd86bea7f83 349
harrynguyen 21:7cd86bea7f83 350 attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
harrynguyen 21:7cd86bea7f83 351 pitch_th, pitch_om, yaw_th , yaw_om,
harrynguyen 21:7cd86bea7f83 352 wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
harrynguyen 21:7cd86bea7f83 353 0, ref_dbeta, ref_beta,
harrynguyen 21:7cd86bea7f83 354 0, ref_dtheta, ref_theta,
harrynguyen 21:7cd86bea7f83 355 0, ref_dgamma, ref_gamma,
harrynguyen 21:7cd86bea7f83 356 &u1, &u2);
harrynguyen 21:7cd86bea7f83 357
harrynguyen 21:7cd86bea7f83 358
harrynguyen 21:7cd86bea7f83 359
harrynguyen 21:7cd86bea7f83 360 if(waypointSetFinish_flag == 0) {
harrynguyen 21:7cd86bea7f83 361 // rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
harrynguyen 21:7cd86bea7f83 362 // rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
harrynguyen 21:7cd86bea7f83 363 rpm_cmd[0]=-800;
harrynguyen 21:7cd86bea7f83 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]);
harrynguyen 21:7cd86bea7f83 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 * nRF network communications as an individual thread
harrynguyen 21:7cd86bea7f83 450 */
harrynguyen 21:7cd86bea7f83 451 void comm_thread(void const *n)
harrynguyen 21:7cd86bea7f83 452 {
harrynguyen 21:7cd86bea7f83 453 comm.init(); //initialize communications unit
harrynguyen 21:7cd86bea7f83 454 Thread::wait(1000); //wait for a bit for radio to complete setup
harrynguyen 21:7cd86bea7f83 455 dataSend_flag=0;
harrynguyen 21:7cd86bea7f83 456
harrynguyen 21:7cd86bea7f83 457 float data[2];
harrynguyen 21:7cd86bea7f83 458 wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0);
harrynguyen 21:7cd86bea7f83 459 //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0);
harrynguyen 21:7cd86bea7f83 460
harrynguyen 21:7cd86bea7f83 461 while(true) {
harrynguyen 21:7cd86bea7f83 462 dataSend_flag =1;
harrynguyen 21:7cd86bea7f83 463
harrynguyen 21:7cd86bea7f83 464 if((dataSend_flag == 1) && (comm.tx_ready == 1)) {
harrynguyen 21:7cd86bea7f83 465
harrynguyen 21:7cd86bea7f83 466 comm.DataOut.addr = 0; //send to node address
harrynguyen 21:7cd86bea7f83 467
harrynguyen 21:7cd86bea7f83 468 comm.DataOut.parameter[0] = 1; //parameter def 0
harrynguyen 21:7cd86bea7f83 469 comm.DataOut.parameter[1] = 2; //parameter def 1
harrynguyen 21:7cd86bea7f83 470
harrynguyen 21:7cd86bea7f83 471 comm.DataOut.dataLen = 20; //length of data to be sent
harrynguyen 21:7cd86bea7f83 472
harrynguyen 21:7cd86bea7f83 473 comm.DataOut.data[0] = imuTime; //timestamp
harrynguyen 21:7cd86bea7f83 474 comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle
harrynguyen 21:7cd86bea7f83 475 comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle
harrynguyen 21:7cd86bea7f83 476 comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle
harrynguyen 21:7cd86bea7f83 477 comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity
harrynguyen 21:7cd86bea7f83 478 comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity
harrynguyen 21:7cd86bea7f83 479 comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity
harrynguyen 21:7cd86bea7f83 480 comm.DataOut.data[7] = imu.LinAcc[0]; //x acc
harrynguyen 21:7cd86bea7f83 481 comm.DataOut.data[8] = imu.LinAcc[1]; //y acc
harrynguyen 21:7cd86bea7f83 482 comm.DataOut.data[9] = imu.LinAcc[2]; //z acc
harrynguyen 21:7cd86bea7f83 483 comm.DataOut.data[10] = localization.position[0]; //localization position x
harrynguyen 21:7cd86bea7f83 484 comm.DataOut.data[11] = localization.position[1]; //localization position y
harrynguyen 21:7cd86bea7f83 485 comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position
harrynguyen 21:7cd86bea7f83 486 comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position
harrynguyen 21:7cd86bea7f83 487 comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity
harrynguyen 21:7cd86bea7f83 488 comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity
harrynguyen 21:7cd86bea7f83 489 comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM %
harrynguyen 21:7cd86bea7f83 490 comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM %
harrynguyen 21:7cd86bea7f83 491 comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command
harrynguyen 21:7cd86bea7f83 492 comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command
harrynguyen 21:7cd86bea7f83 493
harrynguyen 21:7cd86bea7f83 494
harrynguyen 21:7cd86bea7f83 495 comm_status[2] = comm.send();
harrynguyen 21:7cd86bea7f83 496 comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 21:7cd86bea7f83 497 comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 21:7cd86bea7f83 498
harrynguyen 21:7cd86bea7f83 499 if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0
harrynguyen 21:7cd86bea7f83 500 }
harrynguyen 21:7cd86bea7f83 501
harrynguyen 21:7cd86bea7f83 502 else {
harrynguyen 21:7cd86bea7f83 503 comm_status[2] = comm.update();
harrynguyen 21:7cd86bea7f83 504
harrynguyen 21:7cd86bea7f83 505 comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 21:7cd86bea7f83 506 comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 21:7cd86bea7f83 507
harrynguyen 21:7cd86bea7f83 508 if(comm_status[1] == 1) {
harrynguyen 21:7cd86bea7f83 509 //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen);
harrynguyen 21:7cd86bea7f83 510 if(go_cmd == 0) {
harrynguyen 21:7cd86bea7f83 511 if(comm.DataIn.parameter[1] == 0x10) go_cmd=1;
harrynguyen 21:7cd86bea7f83 512 }
harrynguyen 21:7cd86bea7f83 513 }
harrynguyen 21:7cd86bea7f83 514
harrynguyen 21:7cd86bea7f83 515
harrynguyen 21:7cd86bea7f83 516 }
harrynguyen 21:7cd86bea7f83 517
harrynguyen 21:7cd86bea7f83 518 comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 21:7cd86bea7f83 519 comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 21:7cd86bea7f83 520
harrynguyen 21:7cd86bea7f83 521 Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily
harrynguyen 21:7cd86bea7f83 522 }
harrynguyen 21:7cd86bea7f83 523 }
harrynguyen 21:7cd86bea7f83 524
harrynguyen 21:7cd86bea7f83 525
harrynguyen 21:7cd86bea7f83 526 /**
harrynguyen 21:7cd86bea7f83 527 * CameraControl loop as an individual thread
harrynguyen 21:7cd86bea7f83 528 */
harrynguyen 21:7cd86bea7f83 529 void cameraControl_thread(void const *n)
harrynguyen 21:7cd86bea7f83 530 {
harrynguyen 21:7cd86bea7f83 531 cameraFlag=2;
harrynguyen 21:7cd86bea7f83 532 camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0);
harrynguyen 21:7cd86bea7f83 533
harrynguyen 21:7cd86bea7f83 534 while(true) {
harrynguyen 21:7cd86bea7f83 535 if(cameraFlag == 0) {
harrynguyen 21:7cd86bea7f83 536 camera.setState(0);
harrynguyen 21:7cd86bea7f83 537 }
harrynguyen 21:7cd86bea7f83 538
harrynguyen 21:7cd86bea7f83 539 if(cameraFlag == 1) {
harrynguyen 21:7cd86bea7f83 540 camera.updateState();
harrynguyen 21:7cd86bea7f83 541 camera.setState(camera.camFlag);
harrynguyen 21:7cd86bea7f83 542 }
harrynguyen 21:7cd86bea7f83 543
harrynguyen 21:7cd86bea7f83 544 if(cameraFlag == 2) {
harrynguyen 21:7cd86bea7f83 545 camera.setState(1);
harrynguyen 21:7cd86bea7f83 546 }
harrynguyen 21:7cd86bea7f83 547
harrynguyen 21:7cd86bea7f83 548 Thread::wait(100); //proocess thread every 100ms
harrynguyen 21:7cd86bea7f83 549 }
harrynguyen 21:7cd86bea7f83 550 }
harrynguyen 21:7cd86bea7f83 551
harrynguyen 21:7cd86bea7f83 552 /**
harrynguyen 21:7cd86bea7f83 553 * Data recorder loop as an individual thread
harrynguyen 21:7cd86bea7f83 554 */
harrynguyen 21:7cd86bea7f83 555 void dataRecorder_thread(void const *n)
harrynguyen 21:7cd86bea7f83 556 {
harrynguyen 21:7cd86bea7f83 557 virgoRDR.set_size(2);
harrynguyen 21:7cd86bea7f83 558
harrynguyen 21:7cd86bea7f83 559 while(0) {
harrynguyen 21:7cd86bea7f83 560 if(recordFlag == 1) {
harrynguyen 21:7cd86bea7f83 561 virgoRDR.record(imuTime, 0);
harrynguyen 21:7cd86bea7f83 562 virgoRDR.record(imu.Pose[0], 1);
harrynguyen 21:7cd86bea7f83 563 virgoRDR.record(imu.Pose[1], 2);
harrynguyen 21:7cd86bea7f83 564 virgoRDR.record(imu.Pose[2], 3);
harrynguyen 21:7cd86bea7f83 565 virgoRDR.record(imu.AngVel[0], 4);
harrynguyen 21:7cd86bea7f83 566 virgoRDR.record(imu.AngVel[1], 5);
harrynguyen 21:7cd86bea7f83 567 virgoRDR.record(imu.AngVel[2], 6);
harrynguyen 21:7cd86bea7f83 568 virgoRDR.record(localization.position[0], 7);
harrynguyen 21:7cd86bea7f83 569 virgoRDR.record(localization.position[1], 8);
harrynguyen 21:7cd86bea7f83 570 virgoRDR.record(odometry.revolutions[0] * 2*M_PI, 9);
harrynguyen 21:7cd86bea7f83 571 virgoRDR.record(odometry.revolutions[1] * 2*M_PI, 10);
harrynguyen 21:7cd86bea7f83 572 virgoRDR.record(odometry.rpm[0], 11);
harrynguyen 21:7cd86bea7f83 573 virgoRDR.record(odometry.rpm[1], 12);
harrynguyen 21:7cd86bea7f83 574 virgoRDR.record(rpm_cmd[0], 13);
harrynguyen 21:7cd86bea7f83 575 virgoRDR.record(rpm_cmd[1], 14);
harrynguyen 21:7cd86bea7f83 576
harrynguyen 21:7cd86bea7f83 577 virgoRDR.increment_row();
harrynguyen 21:7cd86bea7f83 578
harrynguyen 21:7cd86bea7f83 579 //virgoRDR.size(); //to find number of rows in data recorder
harrynguyen 21:7cd86bea7f83 580 //virgoRDR.current_row(); //current row being recorded to
harrynguyen 21:7cd86bea7f83 581 //data[row][col]; //access stored data
harrynguyen 21:7cd86bea7f83 582 }
harrynguyen 21:7cd86bea7f83 583
harrynguyen 21:7cd86bea7f83 584 Thread::wait(DataRecorder_PeriodMS); //proocess thread every 100ms
harrynguyen 21:7cd86bea7f83 585 }
harrynguyen 21:7cd86bea7f83 586 }
harrynguyen 21:7cd86bea7f83 587
harrynguyen 21:7cd86bea7f83 588 /**
harrynguyen 21:7cd86bea7f83 589 * Debug data print loop as an individual thread
harrynguyen 21:7cd86bea7f83 590 */
harrynguyen 21:7cd86bea7f83 591 #define print_lines 15 //number of info lines being printed on screen
harrynguyen 21:7cd86bea7f83 592 void print_thread(void const *n)
harrynguyen 21:7cd86bea7f83 593 {
harrynguyen 21:7cd86bea7f83 594 //clear 14 lines while going up, these are the initilization lines printed on screen
harrynguyen 21:7cd86bea7f83 595 for(int l=14; l>0; l--) {
harrynguyen 21:7cd86bea7f83 596 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 597 Debug.printf("\e[K"); //clear line
harrynguyen 21:7cd86bea7f83 598 }
harrynguyen 21:7cd86bea7f83 599
harrynguyen 21:7cd86bea7f83 600 Debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
harrynguyen 21:7cd86bea7f83 601 for(int l=print_lines; l>0; l--) Debug.printf("\n");
harrynguyen 21:7cd86bea7f83 602 Debug.printf("\n===================================================");
harrynguyen 21:7cd86bea7f83 603 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 604
harrynguyen 21:7cd86bea7f83 605 while(true) {
harrynguyen 21:7cd86bea7f83 606 //move cursor up # of lines printed to create a static display and clear the first line
harrynguyen 21:7cd86bea7f83 607 for(int l=print_lines; l>0; l--) Debug.printf("\e[1A");
harrynguyen 21:7cd86bea7f83 608 Debug.printf("\e[K");
harrynguyen 21:7cd86bea7f83 609
harrynguyen 21:7cd86bea7f83 610 Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
harrynguyen 21:7cd86bea7f83 611 Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
harrynguyen 21:7cd86bea7f83 612 Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
harrynguyen 21:7cd86bea7f83 613 Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
harrynguyen 21:7cd86bea7f83 614 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 615
harrynguyen 21:7cd86bea7f83 616 //Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
harrynguyen 21:7cd86bea7f83 617
harrynguyen 21:7cd86bea7f83 618 Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
harrynguyen 21:7cd86bea7f83 619 Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE));
harrynguyen 21:7cd86bea7f83 620 Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
harrynguyen 21:7cd86bea7f83 621
harrynguyen 21:7cd86bea7f83 622 Debug.printf("SMC: ref_beta %.2f, ref_dbeta %.3f\n\e[K", RAD_TO_DEG(ref_beta), RAD_TO_DEG(ref_dbeta));
harrynguyen 21:7cd86bea7f83 623 Debug.printf("SMC: ref_gamma %.2f, ref_dgamma %.3f\n\e[K", RAD_TO_DEG(ref_gamma), RAD_TO_DEG(ref_dgamma));
harrynguyen 21:7cd86bea7f83 624 Debug.printf("SMC: ref_theta %.2f, ref_dtheta %.3f\n\e[K", RAD_TO_DEG(ref_theta), RAD_TO_DEG(ref_dtheta));
harrynguyen 21:7cd86bea7f83 625 Debug.printf("SMC: u1*tc %.2f rpm, u2*tc %.2f rpm\n\e[K", u1*0.005*60/(2*M_PI), u2*0.005*60/(2*M_PI));
harrynguyen 21:7cd86bea7f83 626
harrynguyen 21:7cd86bea7f83 627 Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
harrynguyen 21:7cd86bea7f83 628 //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
harrynguyen 21:7cd86bea7f83 629 Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
harrynguyen 21:7cd86bea7f83 630 //Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 21:7cd86bea7f83 631
harrynguyen 21:7cd86bea7f83 632 //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 633 //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 634
harrynguyen 21:7cd86bea7f83 635 Debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready);
harrynguyen 21:7cd86bea7f83 636 //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
harrynguyen 21:7cd86bea7f83 637
harrynguyen 21:7cd86bea7f83 638 Thread::wait(PrintLoop_PeriodMS);
harrynguyen 21:7cd86bea7f83 639 }
harrynguyen 21:7cd86bea7f83 640 }