I've written some code for a class project I'm working on. It functions as expected although it runs much slower than I thought a 16 Mhz clock speed would run it (I'm using the Mega.) A basic explanation of the functions can be found below.
The basic structure can be found in the section titled loop. The purpose of that section is to handle a pin voltage that corresponds to a button that, when pressed, should disable our robot (until it is pressed again.) The goes up for a brief moment and then down, so some logic is needed so that it's not constantly disabling and enabling itself during that time spam. I believe I've handled it efficiently although please tear it apart if it can somehow be made better. The joystick function takes data that's sent from another Xbee (our controller) and translates it into motion. The autonomous1() function isn't included here because I haven't written it yet (it's supposed to guide our robot without any user input for a brief amount of time.) The servo functions operate servos.
Currently, when being operated by the remote, it responds reasonably fast. However, that only came after we gutted some code out to accomplish this. We didn't even remove that much code. For example, 4 simple if statements were previously being used to execute the servo lift/grab functions (instead of directly from the control function.) These 4 simple if statements executed each cycle (they were boolean checks that, if true, would execute the appropriate code.) Their presence introduced a delay of about. 0.25~.5 seconds to every cycle. This just seems outlandish to me given the complexity of the drivers being called in this sketch. They are extremely complex yet execute quicker than my simple series of 4 if/then statements.
Ultimately, I'd like to do a lot with the autonomous function, but at this point I don't even know how much I'll be able to do. The contents will need to be nested in a similar structure to our loop structure to ensure the handling of the disable button is still done in autonomous mode. I have sensors I would like to use and perhaps incorporate into PID functions and the like, but I'm just skeptical of this given how poorly our system responded to the addition of four simple if statements.
Either way, any help would be very well recieved.
/************************************************ INCLUDE LIBRARIES ***************************************************************/
#include "Servo.h"
#include "DualVNH5019MotorShield.h"
#include "LiquidCrystal.h"
LiquidCrystal lcd(24,26,28,30,32,33);
/************************************************* OBJECT INTIALIZATION ***********************************************************/
Servo grabberServo;
Servo liftServo;
DualVNH5019MotorShield md; //Setup the motor driver structure
/*********** CONSTANT VARIABLES ****************/
// XBEE COMMUNICATION
char delimiter = 127;
// INDICATORS
const int red_LED = 23;
const int green_LED = 22;
// SERVOS
const int grab_servo = 29;
const int lift_servo = 27;
// SENSORS
const int easy_button = 31;
const int start_switch = 25;
const int photo_1 = 13; // Set Photoresistor pins: ANALOG
const int photo_2 = 11;
const int photo_3 = 12;
const int range_1 = 15; // Set Rangefinder pins: ANALOG
const int range_2 = 14;
/************************************************* END OBJECT INTIALIZATION **********************************************************/
/************************************************* VARIABLE INTIALIZATION **********************************************************/
char tagged = false; // Start untagged
char tag_first = true;
char autonomous_mode = 1; // Determines which autonomous mode is entered
boolean grabber_open = true; // Begin with the grabber open
boolean arm_down = true; // Begin with the arm down
//********** SOFT CONTROL VARIABLES **********
// XBEE READINGS
char index = 0;
char skip_case = 0;
char incoming_byte = 0;
int joystick_x = 0;
int joystick_y = 0;
char left_button = 0;
char right_button = 0;
char top_button = 0;
char bottom_button = 0;
char select_button = 0;
uint32_t r = 0;
float tan_theta = 0;
// SENSOR READINGS
int photo_1_read = 0;
int photo_2_read = 0;
int photo_3_read = 0;
int range_1_read = 0;
int range_2_read = 0;
//************* HARD CONTROL VARIABLES **************
// REMOTE CONTROL
uint8_t r_dead_zone = 3200;
uint32_t servo_enter_function_time = 0;
const float tan_theta_turn_zone = 0.1405; // Corresponds to 8 degrees
const float tan_theta_straight_zone = 7.115; // Corresponds to 82 degrees
int turbo_mode = false;
// SERVO POSITIONS
const int grabber_open_angle = 3;
const int grabber_close_angle = 130;
const int arm_lift_angle = 180;
const int arm_lower_angle = 3;
// PID CONTROL
const float k_p = 0;
const float k_i = 0;
const float k_d = 0;
//*********** DEBUGGING VARIABLES **********************
boolean debugging = false;
unsigned int debugging_time_step = 500;
unsigned int debugging_current_time = 0;
/******************************************************* END VARIABLE INITIALIZATION *************************************/
/******************************************************* SETUP ***********************************************************/
void setup()
{
pinMode (red_LED, OUTPUT); // Set red LED as output
digitalWrite(red_LED, LOW); // Red LED --> OFF
pinMode (green_LED, OUTPUT); // Set green LED as output
digitalWrite(green_LED, HIGH); // Green LED --> ON
pinMode (easy_button, INPUT); // Set the easy button to be an input
lcd.begin(16, 2); // Specify the columns and rows of the lcd
Serial.begin(9600); // Set the baud rate to 9600 bps for Serial
Serial1.begin(9600); // Set the baud rate to 9600 bps for Serial1
// *************** DEBUGGING *************************
while(debugging == true)
{
}
/*************** END DEBUGGING ******************/
//WAIT FOR AUTONOMOUS MODE BEGIN
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Waiting for");
lcd.setCursor(0,1);
lcd.print("Start Switch");
while(!Serial1.available()) // While nothing is being sent from the remote
{
}
// AUTONOMOUS MODE BEGIN
autonomous_mode = int(Serial1.read()); // Read the autonomous mode from the remote button push
lcd.clear();
lcd.home();
lcd.print(int(autonomous_mode));
Serial1.end();
autonomous_mode_start_time = millis(); // Start the autonomous mode clock
switch(autonomous_mode)
{
case 1:
// 3 SECOND DELAY
lcd.clear();
lcd.setCursor(0,0);
lcd.print("3....");
delay(1000); // Delay 1 second
lcd.clear();
lcd.setCursor(0,0);
lcd.print("2....");
delay(1000); // Delay 1 second
lcd.clear();
lcd.setCursor(0,0);
lcd.print("1....");
delay(1000); // Delay 1 second
lcd.clear();
lcd.setCursor(0,0);
lcd.print("CHEESE IT");
autonomous_mode_1();
break;
}
md.setM1Speed(0);
md.setM2Speed(0);
lcd.clear();
lcd.home();
lcd.print("Set motors to 0");
Serial1.begin(9600);
Serial1.flush(); // Clear the buffer from the incoming serial data
}
/*********************************************** END SETUP ****************************************************/