NXC Program advice
I was wondering if I could get a little advice regarding this program I'm writing. I'm not too worried about it working or compiling or anything like that, but more if there's anything I can do to clean it up or anything else along that line. I've been learning NXC as part of a robotics class, but I have not actually taken a formal coding class, so while I can code, we've never discussed best practices, etc., so I don't feel like I'm necessarily writing the best code. I've posted the code below, and any recommendations would be very much appreciated
Code:
//Motors
#define L_DRIVE OUT_B
#define R_DRIVE OUT_C
#define B_DRIVE OUT_BC
#define ARM OUT_A
//Sensors
#define SONIC SENSOR_1
#define SONIC_PORT S1
#define ARM_TOUCH_PORT S2
#define ARM_TOUCH SENSOR_2
#define BUMPER SENSOR_3
#define BUMPER_PORT S3
#define LIGHT SENSOR_4
#define LIGHT_PORT S4
//Variables
#define CURRENT y
#define START x
#define ORIENTED z
//Constants
#define WALL_DIST 23 //centimeters
//Logging
#define FILE_NAME "data.log"
#define FILE_SIZE 512
//Bluetooth
#define OUTBOX1 1 //ready
#define OUTBOX2 2 //arm is hit
#define INBOX3 3 //partner has ball
#define INBOX4 4 //partner gave ball
#define INBOX5 5 //Right turn for orientation
#define INBOX6 6 //Left turn for orientation
#define CONNECTION 0
#define CONNECTION_GOOD "Connected"
#define CONNECTION_BAD "No connection"
mutex motorMutex;
byte handle;
unsigned int result;
const int file_size = FILE_SIZE;
short bytes_written;
string write, arm, drive, turn;
bool has_ball;
///////////////////
//Arm Subroutines//
///////////////////
void deploy_arm() { //deploys arm to receive ball
arm = "deploying arm";
WriteLnString(handle, arm, bytes_written); //reports status to log
OnFwd(ARM, 65);
Wait(500);
OnRev(ARM, 30); //active braking to reduce impact
Wait(300);
OnRev(ARM, 20);
Wait(200);
Off(ARM);
Wait(2000);
SendRemoteBool(CONNECTION, OUTBOX1, true); //sends ready signal to partner bot
arm = "arm deployed";
WriteLnString(handle, arm, bytes_written); //logfile
}
void drop_arm() { //drops arm for scoring subrout.
OnFwd(ARM, 30); //includes active braking
Wait(500);
OnRev(ARM, 40);
Wait(300);
OnRev(ARM, 20);
Wait(200);
Off(ARM);
Wait(2000);
}
void raise_arm() { //raises arm for scoring subrout
OnRev(ARM, 95);
Wait(1000);
Off(ARM);
Wait(1000);
}
/////////////////////
//Drive Subroutines//
/////////////////////
void forward() {
drive = "moving forward";
WriteLnString(handle, drive, bytes_written);
OnFwd(B_DRIVE, 50);
}
void backward() {
drive = "moving backward";
WriteLnString(handle, drive, bytes_written);
OnRev(B_DRIVE, 50);
}
void rotate_right() {
drive = "turning right";
WriteLnString(handle, drive, bytes_written);
OnFwd(R_DRIVE, 50);
OnRev(L_DRIVE, 50);
}
void rotate_left() {
drive = "turning left";
WriteLnString(handle, drive, bytes_written);
OnFwd(L_DRIVE, 50);
OnRev(R_DRIVE, 50);
}
/////////////////////////
//Bluetooth Subroutines//
/////////////////////////
void receive_color() { //receives data on colored circle from partner bot
bool left, right; //if on red circle, partner sends true on inbox5
ReceiveRemoteBool(INBOX5, true, left); //if on blue circle, partner sends true on inbox 6
ReceiveRemoteBool(INBOX6, true, right); //sub. sets turn direction according to which side is true
until((left == true) || (right == true));
if(right == true) {
turn = "right";
}
else if(left == true) {
turn = "left";
}
}
void receive_ball() { //stops execution until arm bumper is pressed by partner bot
bool clear; //once bumper is pressed, confirmation signal is sent to outbox1
clear = false; //after confirmation, waits for "all clear" signal from partner
until(ButtonPressed(ARM_TOUCH, true));
SendRemoteBool(CONNECTION, OUTBOX1, true);
while(clear = false) {
ReceiveRemoteBool(INBOX4, true, clear);
}
clear = false;
}
///////////////////////
//Complex Subroutines//
///////////////////////
void orient_hoop() { //orients bot towards hoop
bool ORIENTED; //orientation is based off of distance detected on ultrasonic sensor
string orient; //the distance from the hoop wall is shortest, so checks against that distance
orient = "orienting";
WriteLnString(handle, orient, bytes_written);
while(ORIENTED = false) { //loops until oriented
if(SONIC<WALL_DIST) { //checks distance off of ultrasonic sensor
switch(turn) {
case "right": //checks turn direction from receive_color()
rotate_right(); //rotates right to orient
Wait(1000);
Off(B_DRIVE);
break;
case "left": //rotates left to orient
rotate_left();
Wait(1000);
Off(B_DRIVE);
break;
}
}
else if(SONIC>=WALL_DIST) { //finds correct wall
ORIENTED = true; //clears while loop
orient = "oriented";
WriteLnString(handle, orient, bytes_written);
}
}
}
void score() { //raises and lowers arm using respective subs.
string score;
score = "scoring";
WriteLnString(handle, score, bytes_written);
raise_arm();
drop_arm();
}
void drive_hoop() { //drives to the hoop after locating wall
string hoop;
hoop = "driving to hoop";
WriteLnString(handle, hoop, bytes_written);
forward();
Wait(2000);
Off(B_DRIVE);
hoop = "at hoop";
WriteLnString(handle, hoop, bytes_written);
}
/////////
//Tasks//
/////////
task bt_status() { //continually monitors bluetooth connection to partner
string good, bad;
int connection;
connection = BluetoothStatus(CONNECTION);
good = CONNECTION_GOOD;
bad = CONNECTION_BAD;
while(true) {
switch(connection) {
case 0:
TextOut(0, LCD_LINE8, good);
WriteLnString(handle, good, bytes_written);
break;
default:
TextOut(0, LCD_LINE8, bad);
WriteLnString(handle, bad, bytes_written);
break;
}
}
}
task main() {
has_ball = false;
start bt_status;
string write;
result = CreateFile(FILE_NAME, file_size, handle);
if (result == LDR_FILEEXISTS)
result = OpenFileAppend(FILE_NAME, file_size, handle);
if (result == LDR_SUCCESS) {
}
else if (result == LDR_FILEISFULL)
TextOut(0, LCD_LINE1, "file is full");
else {
TextOut(0, LCD_LINE1, "open failed");
TextOut(0, LCD_LINE2, "error");
NumOut(50, LCD_LINE2, result);
}
SetSensorLowspeed(SONIC_PORT);
SetSensorTouch(BUMPER_PORT);
SetSensorTouch(ARM_TOUCH_PORT);
SetSensorLight(LIGHT_PORT);
write = "sensors done";
Wait(5000);
receive_color();
orient_hoop();
drive_hoop();
while(true) {
deploy_arm();
receive_ball();
while(has_ball == false) {
has_ball = false;
score();
ReceiveRemoteBool(INBOX3, true, has_ball);
}
}
}
Also, just for reference, the NXC language is basically C, but with a couple of additional loop statements and APIs that let it play nice with the Lego NXT brick. |
