Alvee Ahmed / Orion_PCB_test_Faulhaber_gr41_wptcmd_V1

Waypoint Command + Obstacle Avoidance + Controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler MAX17048 Servo MODSERIAL

Fork of Orion_PCB_test_Faulhaber_gear_ratio41_waypoint_cmd by Team Virgo v3

main.cpp@5:099cb2e76c7d, 2016-01-25 (annotated)

Committer:
akashvibhute
Date:
Mon Jan 25 07:28:40 2016 +0000
Revision:
5:099cb2e76c7d
Parent:
4:315716ef8178
Child:
6:690db8b5030b
all threads populated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 2:761e3c932ce0 1 /**
akashvibhute 5:099cb2e76c7d 2 * @author Akash Vibhute
akashvibhute 2:761e3c932ce0 3 * < akash . roboticist [at] gmail . com >
akashvibhute 2:761e3c932ce0 4 *
akashvibhute 2:761e3c932ce0 5 * @section LICENSE
akashvibhute 2:761e3c932ce0 6 *
akashvibhute 5:099cb2e76c7d 7 * Copyright (c) 2015 Akash Vibhute
akashvibhute 2:761e3c932ce0 8 *
akashvibhute 2:761e3c932ce0 9 * Licensed under the Apache License, Version 2.0 (the "License");
akashvibhute 2:761e3c932ce0 10 * you may not use this file except in compliance with the License.
akashvibhute 2:761e3c932ce0 11 * You may obtain a copy of the License at
akashvibhute 2:761e3c932ce0 12 *
akashvibhute 2:761e3c932ce0 13 * http://www.apache.org/licenses/LICENSE-2.0
akashvibhute 2:761e3c932ce0 14 *
akashvibhute 2:761e3c932ce0 15 * Unless required by applicable law or agreed to in writing, software
akashvibhute 2:761e3c932ce0 16 * distributed under the License is distributed on an "AS IS" BASIS,
akashvibhute 2:761e3c932ce0 17 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
akashvibhute 2:761e3c932ce0 18 * See the License for the specific language governing permissions and
akashvibhute 2:761e3c932ce0 19 * limitations under the License.
akashvibhute 2:761e3c932ce0 20 *
akashvibhute 2:761e3c932ce0 21 * The above copyright notice and this permission notice shall be included in
akashvibhute 2:761e3c932ce0 22 * all copies or substantial portions of the Software.
akashvibhute 2:761e3c932ce0 23 *
akashvibhute 2:761e3c932ce0 24 * @section DESCRIPTION
akashvibhute 2:761e3c932ce0 25 *
akashvibhute 2:761e3c932ce0 26 * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
akashvibhute 2:761e3c932ce0 27 * Robot controller software for SUTD Virgo version 3 robot
akashvibhute 5:099cb2e76c7d 28 *
akashvibhute 2:761e3c932ce0 29 */
akashvibhute 2:761e3c932ce0 30
akashvibhute 5:099cb2e76c7d 31 /**
akashvibhute 2:761e3c932ce0 32 * Header file for including all necessary functions for robot and defining any
akashvibhute 2:761e3c932ce0 33 * custom functions written in the main file.
akashvibhute 2:761e3c932ce0 34 */
akashvibhute 0:eda518f4a7ae 35 #include "main.h"
akashvibhute 2:761e3c932ce0 36
akashvibhute 5:099cb2e76c7d 37 /**
akashvibhute 5:099cb2e76c7d 38 * Functions, Threads & General Definitions
akashvibhute 5:099cb2e76c7d 39 */
akashvibhute 5:099cb2e76c7d 40 //*****************************************************************************
akashvibhute 5:099cb2e76c7d 41 //** Drivetrain **
akashvibhute 5:099cb2e76c7d 42 motorDriver drive; //motor drive train
akashvibhute 5:099cb2e76c7d 43 odometer odometer; //odometer function
akashvibhute 5:099cb2e76c7d 44 pidControl PID_L, PID_R; //pidcontroller for left and right motors
akashvibhute 0:eda518f4a7ae 45
akashvibhute 5:099cb2e76c7d 46 Timer motorControl_t;
akashvibhute 5:099cb2e76c7d 47 float rpm_cmd[2]; //drive motor rpm command
akashvibhute 5:099cb2e76c7d 48 float pwm_cmd[2]; //drive motor pwm command
akashvibhute 0:eda518f4a7ae 49
akashvibhute 5:099cb2e76c7d 50 /* THREAD */
akashvibhute 5:099cb2e76c7d 51 void odometry_thread(void const *n);
akashvibhute 5:099cb2e76c7d 52 void motorControl_thread(void const *n);
akashvibhute 5:099cb2e76c7d 53 /* ** */
akashvibhute 5:099cb2e76c7d 54 //-------------
akashvibhute 2:761e3c932ce0 55
akashvibhute 5:099cb2e76c7d 56 //** Localization **
akashvibhute 5:099cb2e76c7d 57 IMU_MPU6050 imu; //MPU9150 wrapper class
akashvibhute 5:099cb2e76c7d 58 float imuTime;
akashvibhute 5:099cb2e76c7d 59 localization localization; //localization function
akashvibhute 2:761e3c932ce0 60
akashvibhute 5:099cb2e76c7d 61 /* FUNCTION */
akashvibhute 5:099cb2e76c7d 62 bool imuInit_function();
akashvibhute 5:099cb2e76c7d 63 /* ** */
akashvibhute 5:099cb2e76c7d 64
akashvibhute 5:099cb2e76c7d 65 /* THREAD */
akashvibhute 5:099cb2e76c7d 66 void imu_thread(void const *n);
akashvibhute 5:099cb2e76c7d 67 /* ** */
akashvibhute 5:099cb2e76c7d 68 //-------------
akashvibhute 2:761e3c932ce0 69
akashvibhute 5:099cb2e76c7d 70 //** Communications **
akashvibhute 5:099cb2e76c7d 71 nRF24_Handler nRFcomm; //nRF24 radio and network layer handler function
akashvibhute 5:099cb2e76c7d 72 unsigned int rx_addr, rx_dataLen;
akashvibhute 5:099cb2e76c7d 73 uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
akashvibhute 2:761e3c932ce0 74
akashvibhute 5:099cb2e76c7d 75 /* THREAD */
akashvibhute 5:099cb2e76c7d 76 void commPump_thread(void const *n);
akashvibhute 5:099cb2e76c7d 77 void comm_thread(void const *n);
akashvibhute 5:099cb2e76c7d 78 /* ** */
akashvibhute 5:099cb2e76c7d 79 //-------------
akashvibhute 2:761e3c932ce0 80
akashvibhute 5:099cb2e76c7d 81 //** Power Monitor **
akashvibhute 5:099cb2e76c7d 82 BattGuage battery; //Battery fuel gauge wrapper
akashvibhute 2:761e3c932ce0 83
akashvibhute 5:099cb2e76c7d 84 /* THREAD */
akashvibhute 2:761e3c932ce0 85
akashvibhute 5:099cb2e76c7d 86 /* ** */
akashvibhute 5:099cb2e76c7d 87 //-------------
akashvibhute 2:761e3c932ce0 88
akashvibhute 5:099cb2e76c7d 89 //** Trajectory tracking **
akashvibhute 5:099cb2e76c7d 90 purePursuit purePursuit;
akashvibhute 5:099cb2e76c7d 91 Timer camera_t;
akashvibhute 2:761e3c932ce0 92
akashvibhute 5:099cb2e76c7d 93 /* THREAD */
akashvibhute 5:099cb2e76c7d 94 void purePursuit_thread(void const *n);
akashvibhute 5:099cb2e76c7d 95 void waypointCmd_thread(void const *n);
akashvibhute 5:099cb2e76c7d 96 /* ** */
akashvibhute 5:099cb2e76c7d 97 //-------------
akashvibhute 2:761e3c932ce0 98
akashvibhute 5:099cb2e76c7d 99 //** Attitude control **
akashvibhute 5:099cb2e76c7d 100 attitudeControl attitudeControl;
akashvibhute 2:761e3c932ce0 101
akashvibhute 5:099cb2e76c7d 102 /* THREAD */
akashvibhute 2:761e3c932ce0 103
akashvibhute 5:099cb2e76c7d 104 /* ** */
akashvibhute 5:099cb2e76c7d 105 //-------------
akashvibhute 2:761e3c932ce0 106
akashvibhute 5:099cb2e76c7d 107 //** Camera **
akashvibhute 5:099cb2e76c7d 108 camera camera; //camera driver
akashvibhute 5:099cb2e76c7d 109 bool cameraFlag; //flag to enable camera
akashvibhute 2:761e3c932ce0 110
akashvibhute 5:099cb2e76c7d 111 /* THREAD */
akashvibhute 5:099cb2e76c7d 112 void cameraControl_thread(void const *n);
akashvibhute 5:099cb2e76c7d 113 /* ** */
akashvibhute 5:099cb2e76c7d 114 //-------------
akashvibhute 2:761e3c932ce0 115
akashvibhute 5:099cb2e76c7d 116 //** Robot data recorder **
akashvibhute 5:099cb2e76c7d 117 //dataRecorder virgoRDR;
akashvibhute 2:761e3c932ce0 118
akashvibhute 5:099cb2e76c7d 119 /* THREAD */
akashvibhute 2:761e3c932ce0 120
akashvibhute 5:099cb2e76c7d 121 /* ** */
akashvibhute 5:099cb2e76c7d 122 //-------------
akashvibhute 2:761e3c932ce0 123
akashvibhute 5:099cb2e76c7d 124 //** Declarations of misc functions **
akashvibhute 5:099cb2e76c7d 125 DigitalOut debugLED(debug_LED); //led for debugging and heartbeat indication
akashvibhute 5:099cb2e76c7d 126 Serial debug(uart_TX, uart_RX); //debug serial port
akashvibhute 5:099cb2e76c7d 127 USBSerial usb; //USB virtual serial port
akashvibhute 2:761e3c932ce0 128
akashvibhute 5:099cb2e76c7d 129 /* THREAD */
akashvibhute 5:099cb2e76c7d 130 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
akashvibhute 5:099cb2e76c7d 131 void print_thread(void const *n); //debug printing thread
akashvibhute 5:099cb2e76c7d 132 /* ** */
akashvibhute 5:099cb2e76c7d 133 //-------------
akashvibhute 5:099cb2e76c7d 134 //-----------------------------------------------------------------------------
akashvibhute 2:761e3c932ce0 135
akashvibhute 2:761e3c932ce0 136
akashvibhute 5:099cb2e76c7d 137 int main()
akashvibhute 2:761e3c932ce0 138 {
akashvibhute 4:315716ef8178 139 debug.baud(PC_BAUDRATE);
akashvibhute 4:315716ef8178 140 debugLED =1;
akashvibhute 4:315716ef8178 141 drive.setPWM_L(0);
akashvibhute 4:315716ef8178 142 drive.setPWM_R(0);
akashvibhute 2:761e3c932ce0 143
akashvibhute 5:099cb2e76c7d 144 debug.printf("** Starting Virgo v3 Routines *************\n\n");
akashvibhute 2:761e3c932ce0 145
akashvibhute 5:099cb2e76c7d 146 //** start Hearbeat loop as a thread **
akashvibhute 5:099cb2e76c7d 147 Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 148 debug.printf("* Hearbeat loop started *\n");
akashvibhute 5:099cb2e76c7d 149
akashvibhute 5:099cb2e76c7d 150 //** start IMU funtion as Thread **
akashvibhute 5:099cb2e76c7d 151 Thread IMU_function(imu_thread, NULL, osPriorityHigh);
akashvibhute 5:099cb2e76c7d 152 debug.printf("* IMU routine started *\n");
akashvibhute 5:099cb2e76c7d 153
akashvibhute 5:099cb2e76c7d 154 //** start OdometryUpdate function as Thread **
akashvibhute 5:099cb2e76c7d 155 Thread Odometry_function(odometry_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 156 debug.printf("* Odometry routine started *\n");
akashvibhute 5:099cb2e76c7d 157
akashvibhute 5:099cb2e76c7d 158 //** start MotorControl function as Thread **
akashvibhute 5:099cb2e76c7d 159 Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 160 debug.printf("* Motor control routine started *\n");
akashvibhute 2:761e3c932ce0 161
akashvibhute 5:099cb2e76c7d 162 //** start PurePursuit controller as Thread **
akashvibhute 5:099cb2e76c7d 163 Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 164 debug.printf("* PurePursuit controller routine started *\n");
akashvibhute 5:099cb2e76c7d 165
akashvibhute 5:099cb2e76c7d 166 //** start Waypoint commander function as Thread **
akashvibhute 5:099cb2e76c7d 167 Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 168 debug.printf("* Waypoint commander routine started *\n");
akashvibhute 5:099cb2e76c7d 169
akashvibhute 5:099cb2e76c7d 170 //** start comm loop as a thread **
akashvibhute 5:099cb2e76c7d 171 nRFcomm.commInit();
akashvibhute 5:099cb2e76c7d 172 Thread CommPump_function(commPump_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 173 Thread CommLoop_function(comm_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 174 debug.printf("* Communications loop started *\n");
akashvibhute 5:099cb2e76c7d 175
akashvibhute 5:099cb2e76c7d 176 //** start camera control loop as a thread **
akashvibhute 5:099cb2e76c7d 177 Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 178 debug.printf("* Camera control loop started *\n");
akashvibhute 5:099cb2e76c7d 179
akashvibhute 5:099cb2e76c7d 180 //** start debug print loop as a thread **
akashvibhute 5:099cb2e76c7d 181 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal);
akashvibhute 5:099cb2e76c7d 182 debug.printf("* Print loop started *\n\n\n");
akashvibhute 5:099cb2e76c7d 183
akashvibhute 5:099cb2e76c7d 184 debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
akashvibhute 2:761e3c932ce0 185
akashvibhute 5:099cb2e76c7d 186 while(1) {
akashvibhute 5:099cb2e76c7d 187
akashvibhute 0:eda518f4a7ae 188 }
akashvibhute 2:761e3c932ce0 189 }
akashvibhute 2:761e3c932ce0 190
akashvibhute 5:099cb2e76c7d 191 /**
akashvibhute 5:099cb2e76c7d 192 * heartbeat loop as an individual thread
akashvibhute 5:099cb2e76c7d 193 */
akashvibhute 5:099cb2e76c7d 194 void heartbeat_thread(void const *n)
akashvibhute 2:761e3c932ce0 195 {
akashvibhute 2:761e3c932ce0 196 while(true) {
akashvibhute 2:761e3c932ce0 197 debugLED = !debugLED;
akashvibhute 2:761e3c932ce0 198 Thread::wait(Hearbeat_RateMS);
akashvibhute 2:761e3c932ce0 199 }
akashvibhute 2:761e3c932ce0 200 }
akashvibhute 2:761e3c932ce0 201
akashvibhute 5:099cb2e76c7d 202 /**
akashvibhute 5:099cb2e76c7d 203 * imu initialization function
akashvibhute 5:099cb2e76c7d 204 */
akashvibhute 5:099cb2e76c7d 205 bool imuInit_function()
akashvibhute 5:099cb2e76c7d 206 {
akashvibhute 5:099cb2e76c7d 207 return (imu.imuInit());
akashvibhute 5:099cb2e76c7d 208 }
akashvibhute 5:099cb2e76c7d 209
akashvibhute 5:099cb2e76c7d 210 /**
akashvibhute 5:099cb2e76c7d 211 * imu update loop as an individual thread
akashvibhute 5:099cb2e76c7d 212 */
akashvibhute 5:099cb2e76c7d 213 void imu_thread(void const *n)
akashvibhute 2:761e3c932ce0 214 {
akashvibhute 5:099cb2e76c7d 215 while(true) {
akashvibhute 5:099cb2e76c7d 216 imu.imuUpdate();
akashvibhute 5:099cb2e76c7d 217
akashvibhute 5:099cb2e76c7d 218 //imu.Pose[0, 1, 2]; //euler x, y, z
akashvibhute 5:099cb2e76c7d 219 //imu.AngVel[0, 1, 2]; //AngVel x, y, z
akashvibhute 5:099cb2e76c7d 220 //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
akashvibhute 5:099cb2e76c7d 221 //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
akashvibhute 5:099cb2e76c7d 222
akashvibhute 5:099cb2e76c7d 223 imuTime = imu.time_s;
akashvibhute 5:099cb2e76c7d 224
akashvibhute 5:099cb2e76c7d 225 Thread::wait(imu_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 226 }
akashvibhute 5:099cb2e76c7d 227 }
akashvibhute 5:099cb2e76c7d 228
akashvibhute 5:099cb2e76c7d 229 /**
akashvibhute 5:099cb2e76c7d 230 * odometry update loop as an individual thread
akashvibhute 5:099cb2e76c7d 231 */
akashvibhute 5:099cb2e76c7d 232 void odometry_thread(void const *n)
akashvibhute 5:099cb2e76c7d 233 {
akashvibhute 5:099cb2e76c7d 234 while(true) {
akashvibhute 5:099cb2e76c7d 235 odometer.update();
akashvibhute 5:099cb2e76c7d 236
akashvibhute 5:099cb2e76c7d 237 //odometer.revolutions[0, 1]; //revolutions left, right
akashvibhute 5:099cb2e76c7d 238 //odometer.rpm[0, 1]; //rpm left, right
akashvibhute 5:099cb2e76c7d 239
akashvibhute 5:099cb2e76c7d 240 Thread::wait(odometry_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 241 }
akashvibhute 5:099cb2e76c7d 242 }
akashvibhute 5:099cb2e76c7d 243
akashvibhute 5:099cb2e76c7d 244 /**
akashvibhute 5:099cb2e76c7d 245 * motor control loop as an individual thread
akashvibhute 5:099cb2e76c7d 246 */
akashvibhute 5:099cb2e76c7d 247 void motorControl_thread(void const *n)
akashvibhute 5:099cb2e76c7d 248 {
akashvibhute 5:099cb2e76c7d 249 motorControl_t.start();
akashvibhute 5:099cb2e76c7d 250
akashvibhute 2:761e3c932ce0 251 while(true) {
akashvibhute 5:099cb2e76c7d 252 /*
akashvibhute 5:099cb2e76c7d 253 attitudeControl.GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
akashvibhute 5:099cb2e76c7d 254 float beta, float dbeta, float gamma, float dgamma,
akashvibhute 5:099cb2e76c7d 255 float theta_L, float theta_R, float dtheta_L, float dtheta_R,
akashvibhute 5:099cb2e76c7d 256 float ref_ddbeta, float ref_dbeta, float ref_beta,
akashvibhute 5:099cb2e76c7d 257 float ref_ddtheta, float ref_dtheta, float ref_theta,
akashvibhute 5:099cb2e76c7d 258 float ref_ddgamma, float ref_dgamma, float ref_gamma);
akashvibhute 5:099cb2e76c7d 259 */
akashvibhute 5:099cb2e76c7d 260
akashvibhute 5:099cb2e76c7d 261 pwm_cmd[0]=PID_L.calcOutput(rpm_cmd[0], odometer.rpm[0], motorControl_t.read());
akashvibhute 5:099cb2e76c7d 262 pwm_cmd[1]=PID_R.calcOutput(rpm_cmd[1], odometer.rpm[1], motorControl_t.read());
akashvibhute 5:099cb2e76c7d 263
akashvibhute 5:099cb2e76c7d 264 motorControl_t.reset();
akashvibhute 5:099cb2e76c7d 265
akashvibhute 5:099cb2e76c7d 266 drive.setPWM_L(pwm_cmd[0]);
akashvibhute 5:099cb2e76c7d 267 drive.setPWM_R(pwm_cmd[1]);
akashvibhute 5:099cb2e76c7d 268
akashvibhute 5:099cb2e76c7d 269 Thread::wait(motorControl_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 270 }
akashvibhute 5:099cb2e76c7d 271 }
akashvibhute 5:099cb2e76c7d 272
akashvibhute 5:099cb2e76c7d 273 /**
akashvibhute 5:099cb2e76c7d 274 * pure pursuit loop as an individual thread
akashvibhute 5:099cb2e76c7d 275 */
akashvibhute 5:099cb2e76c7d 276 void purePursuit_thread(void const *n)
akashvibhute 5:099cb2e76c7d 277 {
akashvibhute 5:099cb2e76c7d 278 while(true) {
akashvibhute 5:099cb2e76c7d 279 /*
akashvibhute 5:099cb2e76c7d 280 purePursuit.GenVW(float* purePursuit_velocity, float* purePursuit_omega,
akashvibhute 5:099cb2e76c7d 281 float target_waypoint[2], float target_velocity,
akashvibhute 5:099cb2e76c7d 282 float current_position[2], float current_heading);
akashvibhute 5:099cb2e76c7d 283 */
akashvibhute 5:099cb2e76c7d 284 Thread::wait(PurePursuit_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 285 }
akashvibhute 5:099cb2e76c7d 286 }
akashvibhute 5:099cb2e76c7d 287
akashvibhute 5:099cb2e76c7d 288 /**
akashvibhute 5:099cb2e76c7d 289 * waypoint command update loop as an individual thread
akashvibhute 5:099cb2e76c7d 290 */
akashvibhute 5:099cb2e76c7d 291 void waypointCmd_thread(void const *n)
akashvibhute 5:099cb2e76c7d 292 {
akashvibhute 5:099cb2e76c7d 293 while(true) {
akashvibhute 5:099cb2e76c7d 294
akashvibhute 5:099cb2e76c7d 295 Thread::wait(WaypointCmd_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 296 }
akashvibhute 5:099cb2e76c7d 297 }
akashvibhute 5:099cb2e76c7d 298
akashvibhute 5:099cb2e76c7d 299 /**
akashvibhute 5:099cb2e76c7d 300 * nRF network communications pump as an individual thread
akashvibhute 5:099cb2e76c7d 301 */
akashvibhute 5:099cb2e76c7d 302 void commPump_thread(void const *n)
akashvibhute 5:099cb2e76c7d 303 {
akashvibhute 5:099cb2e76c7d 304 while(true) {
akashvibhute 5:099cb2e76c7d 305 nRFcomm.commUpdate();
akashvibhute 5:099cb2e76c7d 306
akashvibhute 5:099cb2e76c7d 307 Thread::wait(CommPump_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 308 }
akashvibhute 5:099cb2e76c7d 309 }
akashvibhute 5:099cb2e76c7d 310
akashvibhute 5:099cb2e76c7d 311 /**
akashvibhute 5:099cb2e76c7d 312 * communications loop as an individual thread
akashvibhute 5:099cb2e76c7d 313 */
akashvibhute 5:099cb2e76c7d 314 void comm_thread(void const *n)
akashvibhute 5:099cb2e76c7d 315 {
akashvibhute 5:099cb2e76c7d 316 dataSend_flag=0;
akashvibhute 5:099cb2e76c7d 317
akashvibhute 5:099cb2e76c7d 318 while(true) {
akashvibhute 5:099cb2e76c7d 319 if(nRFcomm.commReceive(&rx_addr, &rx_dataLen)) {
akashvibhute 5:099cb2e76c7d 320 //nRFcomm.parameter_in[0, 1];
akashvibhute 5:099cb2e76c7d 321 //nRFcomm.data_in[0, 1, ..., n];
akashvibhute 2:761e3c932ce0 322
akashvibhute 5:099cb2e76c7d 323 //process received data
akashvibhute 5:099cb2e76c7d 324 }
akashvibhute 5:099cb2e76c7d 325
akashvibhute 5:099cb2e76c7d 326 if(dataSend_flag) {
akashvibhute 5:099cb2e76c7d 327 //nRFcomm.commSend(unsigned int toAddress, unsigned int parameter[2], float data[], unsigned int dataLen);
akashvibhute 5:099cb2e76c7d 328 }
akashvibhute 5:099cb2e76c7d 329
akashvibhute 5:099cb2e76c7d 330 Thread::wait(CommLoop_UpdatePeriodMS);
akashvibhute 5:099cb2e76c7d 331 }
akashvibhute 5:099cb2e76c7d 332 }
akashvibhute 5:099cb2e76c7d 333
akashvibhute 5:099cb2e76c7d 334 /**
akashvibhute 5:099cb2e76c7d 335 * CameraControl loop as an individual thread
akashvibhute 5:099cb2e76c7d 336 */
akashvibhute 5:099cb2e76c7d 337 void cameraControl_thread(void const *n)
akashvibhute 5:099cb2e76c7d 338 {
akashvibhute 5:099cb2e76c7d 339 cameraFlag=0;
akashvibhute 5:099cb2e76c7d 340 camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0);
akashvibhute 5:099cb2e76c7d 341
akashvibhute 5:099cb2e76c7d 342 while(true) {
akashvibhute 5:099cb2e76c7d 343 camera.updateState();
akashvibhute 5:099cb2e76c7d 344 camera.setState(camera.camFlag);
akashvibhute 5:099cb2e76c7d 345
akashvibhute 2:761e3c932ce0 346 Thread::wait(100); //proocess thread every 100ms
akashvibhute 2:761e3c932ce0 347 }
akashvibhute 2:761e3c932ce0 348 }
akashvibhute 2:761e3c932ce0 349
akashvibhute 5:099cb2e76c7d 350 /**
akashvibhute 5:099cb2e76c7d 351 * debug data print loop as an individual thread
akashvibhute 5:099cb2e76c7d 352 */
akashvibhute 2:761e3c932ce0 353 #define print_lines 2 //number of info lines being printed on screen
akashvibhute 5:099cb2e76c7d 354 void print_thread(void const *n)
akashvibhute 2:761e3c932ce0 355 {
akashvibhute 5:099cb2e76c7d 356 while(!imu.imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
akashvibhute 0:eda518f4a7ae 357
akashvibhute 2:761e3c932ce0 358 //clear 14 lines while going up, these are the initilization lines printed on screen
akashvibhute 2:761e3c932ce0 359 for(int l=14; l>0; l--) {
akashvibhute 5:099cb2e76c7d 360 debug.printf("\e[1A"); //go up 1 line
akashvibhute 5:099cb2e76c7d 361 debug.printf("\e[K"); //clear line
akashvibhute 2:761e3c932ce0 362 }
akashvibhute 2:761e3c932ce0 363
akashvibhute 4:315716ef8178 364 printf("************ VIRGO v3: Status Monitor *************\n\n");
akashvibhute 4:315716ef8178 365 for(int l=print_lines; l>0; l--) printf("\n");
akashvibhute 4:315716ef8178 366 printf("\n===================================================");
akashvibhute 4:315716ef8178 367 printf("\e[1A"); //go up 1 line
akashvibhute 2:761e3c932ce0 368
akashvibhute 2:761e3c932ce0 369 while(true) {
akashvibhute 2:761e3c932ce0 370 //move cursor up # of lines printed to create a static display and clear the first line
akashvibhute 4:315716ef8178 371 for(int l=print_lines; l>0; l--) printf("\e[1A");
akashvibhute 4:315716ef8178 372 printf("\e[K");
akashvibhute 2:761e3c932ce0 373
akashvibhute 2:761e3c932ce0 374 //mutex_PrintLoopThread.lock();
akashvibhute 2:761e3c932ce0 375
akashvibhute 2:761e3c932ce0 376
akashvibhute 5:099cb2e76c7d 377 printf("Elapsed time: %.2f s\n\e[K", imuTime);
akashvibhute 2:761e3c932ce0 378 //debug.printf("Status: Heading %.2f, CmdRPM <%.1f , %.1f>\n\e[K", RAD_TO_DEG(imu_Data.yaw), motor_CmdRPM[0], motor_CmdRPM[1]);
akashvibhute 2:761e3c932ce0 379 //debug.printf("Orientation (YPR): (%.2f , %.2f , %.2f)\n\e[K", RAD_TO_DEG(imu_Data.yaw), RAD_TO_DEG(imu_Data.pitch), RAD_TO_DEG(imu_Data.roll));
akashvibhute 2:761e3c932ce0 380 //debug.printf("Command: Position <%.1f , %.1f> \tVelocity <%.1f>\n\e[K", waypoint_target[0], waypoint_target[1], waypoint_target[2]);
akashvibhute 2:761e3c932ce0 381 //debug.printf("Current: Position <%.1f , %.1f> \tVelocity <%.1f>\n\e[K", robot_pos[0], robot_pos[1], (drive_velocity[0]+drive_velocity[1])/2);
akashvibhute 2:761e3c932ce0 382 //debug.printf("Distance to go: %.1f\n\e[K", robotFrame_target[2]);
akashvibhute 2:761e3c932ce0 383 //debug.printf("Command RPM (L,R): %.1f, %.1f\n\e[K", motor_CmdRPM[0], motor_CmdRPM[1]);
akashvibhute 2:761e3c932ce0 384 //debug.printf("Received Command 0x%x :: <%.1f, %.1f, %.1f, %.1f>\n\e[K", RxData.cmd, RxData.RxVector[0], RxData.RxVector[1], RxData.RxVector[2], RxData.RxVector[3]);
akashvibhute 2:761e3c932ce0 385
akashvibhute 2:761e3c932ce0 386 //debug.printf("Controller params: \n\e[K");
akashvibhute 2:761e3c932ce0 387 //debug.printf("\tW_l: %.1f, W_r: %.1f\n\e[K", W_l, W_r);
akashvibhute 2:761e3c932ce0 388 //debug.printf("\tpitch_th: %.1f, pitch_om: %.1f, yaw_th: %.1f, yaw_om: %.1f,\n\e[K", pitch_th, pitch_om, yaw_th, yaw_om);
akashvibhute 2:761e3c932ce0 389 //debug.printf("\twheelTh_l: %.1f, wheelTh_r: %.1f, wheelOm_l: %.1f, wheelOm_r: %.1f\n\e[K", wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r);
akashvibhute 2:761e3c932ce0 390
akashvibhute 2:761e3c932ce0 391
akashvibhute 2:761e3c932ce0 392 //debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry[0][1]*60, odometry[1][1]*60);
akashvibhute 2:761e3c932ce0 393 //debug.printf("PurePursuit: ld %.1f, r %.1f\n\e[K", purePursuit_ld, purePursuit_r);
akashvibhute 2:761e3c932ce0 394 //debug.printf("PurePursuit: hdgErr %.1f, omega %.1f\n\e[K", RAD_TO_DEG(purePursuit_headingE), RAD_TO_DEG(purePursuit_omega));
akashvibhute 2:761e3c932ce0 395 //debug.printf("PWM Percent (L,R): %.2f, %.2f\n\e[K", PIDFfS_terms[0][4]*100.0, PIDFfS_terms[1][4]*100.0);
akashvibhute 2:761e3c932ce0 396 //debug.printf("Error RPM (L,R): %.1f, %.1f\n\e[K", motor_ControlError[0][1], motor_ControlError[1][1]);
akashvibhute 2:761e3c932ce0 397 //debug.printf("Individual terms (Percent) P= %.2f, I= %.2f, D= %.2f, FF= %.2f\n", PIDFfS_terms[0][0]*100.0, PIDFfS_terms[0][1]*100.0, PIDFfS_terms[0][2]*100.0, PIDFfS_terms[0][3]*100.0);
akashvibhute 2:761e3c932ce0 398 //debug.printf("%.2f:%.2f,%.2f,%.2f,%.2f:%.2f\n", odometry[0][1]*60,PIDFfS_terms[0][0]*100.0, PIDFfS_terms[0][1]*100.0, PIDFfS_terms[0][2]*100.0, PIDFfS_terms[0][3]*100.0,PIDFfS_terms[0][4]*100.0);
akashvibhute 2:761e3c932ce0 399 //debug.printf("%.2f %.2f %.2f %.2f\n", motor_CmdRPM[0], odometry[0][1]*60, PIDFfS_terms[0][4]*100.0, PIDFfS_terms[0][5]*100.0);
akashvibhute 2:761e3c932ce0 400
akashvibhute 2:761e3c932ce0 401 //mutex_PrintLoopThread.unlock();
akashvibhute 2:761e3c932ce0 402
akashvibhute 2:761e3c932ce0 403 Thread::wait(PrintLoop_PeriodMS);
akashvibhute 2:761e3c932ce0 404 }
akashvibhute 5:099cb2e76c7d 405 }