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
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?
User | Revision | Line number | New 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 | } |