int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
if (Drive_init() != SUCCESS) {
printf("Failed to initialize drive module.\n");
}
I2C_init(TILT_COMPASS_I2C_ID, I2C_CLOCK_FREQ);
if (TiltCompass_init() != SUCCESS) {
printf("Failed to initialize tilt compass module.\n");
}
ENABLE_OUT_TRIS = OUTPUT;
while (1) {
printf("Driving north at full speed.\n");
Timer_new(TIMER_TEST, COMMAND_DELAY);
Drive_forwardHeading(100, 0);
while(!Timer_isExpired(TIMER_TEST)) {
//wait for finish
DELAY(1);
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
Drive_runSM();
TiltCompass_runSM();
printf("Driving south at full speed.\n");
Timer_new(TIMER_TEST, COMMAND_DELAY);
Drive_forwardHeading(100, 180);
while(!Timer_isExpired(TIMER_TEST)) {
//wait for finish
DELAY(1);
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
Drive_runSM();
TiltCompass_runSM();
delayMillisecond(COMMAND_DELAY);
printf("Waiting for retry...\n");
}
return SUCCESS;
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
Drive_init();
ENABLE_OUT_TRIS = OUTPUT;
ENABLE_OUT_LAT = MICRO;
#ifdef RECIEVE_CONTROL
ENABLE_OUT_LAT = RECIEVER;
while(1){
asm("nop");
}
#endif
printf("State machine test harness.\n\n");
// Test forward state
printf("Forward state. Driving at 5, 10, 20, 40, 60, 80, 100, 125 \%.\n");
uint8_t speed[] = {5, 10, 20, 40, 60, 80, 100, 125};
int i, len = sizeof(speed);
delayMillisecond(STATE_DELAY);
for (i = 0; i < len; i++) {
Drive_forward(speed[i]);
delayMillisecond(STATE_DELAY);
}
Drive_stop();
delayMillisecond(STATE_DELAY);
setRudder(STOP_PULSE);
delayMillisecond(STATE_DELAY);
// Forward with heading
printf("Track state. Driving at 5, 10, 25, 50, 100 \% at 0 deg north.\n");
uint8_t speed2[] = {5, 10, 25, 50, 100};
len = sizeof(speed2);
delayMillisecond(STATE_DELAY);
for (i = 0; i < len; i++) {
Drive_forwardHeading(speed2[i], 0);
delayMillisecond(STATE_DELAY);
}
Drive_stop();
printf("Finished state machine test.\n\n");
return SUCCESS;
}
int main() {
ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates
ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller
Board_init();
Serial_init();
Timer_init();
Drive_init();
I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ);
TiltCompass_init();
printf("Boat initialized.\n");
Drive_stop();
DELAY(5);
#ifdef MOTOR_TEST
Drive_pivot(0);
#endif
#ifdef RUDDER_TEST
Drive_forwardHeading(0.0, desiredHeading);
#endif
int i = 0;
int velocity[] = {1600, 1600, 1600, 1600, 1600};
velocityPulse = velocity[i];
Timer_new(TIMER_TEST,FINISH_DELAY);
while (1) {
if (Timer_isExpired(TIMER_TEST))
{
i++;
if (i == 5){
i = 0;
}
velocityPulse = velocity[i];
printf("CURRENT VELOCITY: %d\n\n",velocityPulse);
Timer_new(TIMER_TEST,FINISH_DELAY);
}
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
return SUCCESS;
}
/**********************************************************************
* Function: Override_giveReceiverControl
* @return None
* @remark Passes motor control over to the receiver.
* @author David Goodman
* @date 2013.04.01
**********************************************************************/
void Override_giveReceiverControl() {
//DBPRINT("Reciever has control\n");
#ifdef USE_DRIVE
Drive_stop();
#endif
//Give control over to Reciever using the enable line
ENABLE_OUT_LAT = RECIEVER_HAS_CONTROL;
}
/**********************************************************************
* Function: Override_giveMicroControl
* @return None
* @remark Passes motor control over to the micro.
* @author David Goodman
* @date 2013.04.01
**********************************************************************/
void Override_giveMicroControl() {
//DBPRINT("Micro has control\n");
#ifdef USE_DRIVE
Drive_stop();
#endif
//Give control over to Micro using the enable line
ENABLE_OUT_LAT = MICRO_HAS_CONTROL;
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
Drive_init();
int spd = 10;
ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates
ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller
enum{
MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward
MICRO_STOP = 0x02, //State where Micro is driving backwards
MICRO_LIMBO = 0x03,
RECIEVER_STATE = 0x04, //Reciever has taken over state
}test_state;
printf("Boat is Initialized\n");
//Initialize the state to begin at forward
test_state = MICRO_FORWARD;
Drive_stop();
Timer_new(TIMER_TEST,RECIEVER_DELAY);
int state_flag = 0;
Override_init();
while(1){
switch(test_state){
case MICRO_FORWARD:
printf("STATE: MICRO_FORWARD\n\n\n");
Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_STOP;
break;
case MICRO_STOP:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_STOP\n\n\n:");
Drive_stop();
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_LIMBO;
}
break;
case MICRO_LIMBO:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_LIMBO\n\n\n");
test_state = MICRO_FORWARD;
}
Drive_stop();
break;
case RECIEVER_STATE:
;
break;
}
//If we got a pulse, control to reciever.
if (OVERRIDE_TRIGGERED == TRUE){
printf("Reciever Control\n\n");
Timer_new(TIMER_TEST, 1000); //Set timer that is greater than the pulsewidth of the CH3 signal(54Hz)
OVERRIDE_TRIGGERED = FALSE; //Re-init to zero so that we know when our pulse is triggered again.
test_state = RECIEVER_STATE; //Set state equal to reciever where we do nothing autonomous
ENABLE_OUT_LAT = RECIEVER; //Give control over to Reciever using the enable line
INTEnable(INT_CN,1);
}
if (Timer_isExpired(TIMER_TEST)){ //Reciever gave up control
printf("Micro Has Control\n\n");
Timer_clear(TIMER_TEST); //Clear timer so that it doesn't keep registering an expired signal
test_state = MICRO_LIMBO; //Set state equal to forward for regular function
OVERRIDE_TRIGGERED = FALSE; //Set Override to false to be sure that we don't trigger falsely
ENABLE_OUT_LAT = MICRO; //Give Control back to microcontroller using enable line
Timer_new(TIMER_TEST2, RECIEVER_DELAY);
}
}
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
Drive_init();
int spd = 10;
ENABLE_TRIS = INPUT; //Set the direction of the Enable pin to be an input, a switch will be used for now
ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates
ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller
enum{
MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward
MICRO_STOP = 0x02, //State where Micro is driving backwards
MICRO_LIMBO = 0x03,
RECIEVER_STATE = 0x04, //Reciever has taken over state
}test_state;
printf("Boat is Initialized\n");
//Initialize the state to begin at forward
test_state = MICRO_FORWARD;
Drive_stop();
Timer_new(TIMER_TEST,RECIEVER_DELAY);
int state_flag = 0;
while(1){
if(Timer_isExpired(TIMER_TEST)){
if ((ENABLE_BIT == 1)){
test_state = RECIEVER_STATE;
printf("YOU HAVE TRIGGERED THE RECIEVER FOR CONTROL\n\n");
ENABLE_OUT_LAT = RECIEVER; //Set the Enable_out pin that will be routed to the AND gates
state_flag = 1;
}else if (ENABLE_BIT == 0){
printf("MICRO HAS CONTROL\n\n");
ENABLE_OUT_LAT = MICRO; //setting Enable_out that will be routed to AND gates
if(state_flag == 1){
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_FORWARD;
state_flag = 0;
}
}
Timer_new(TIMER_TEST,RECIEVER_DELAY);
}
switch(test_state){
case MICRO_FORWARD:
printf("STATE: MICRO_FORWARD\n\n\n");
Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_STOP;
break;
case MICRO_STOP:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_STOP\n\n\n:");
Drive_stop();
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_LIMBO;
}
break;
case MICRO_LIMBO:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_LIMBO\n\n\n");
test_state = MICRO_FORWARD;
}
Drive_stop();
break;
case RECIEVER_STATE:
;
break;
}
}
}
int main() {
enum{
forward = 0x01,
backward = 0x02,
idle = 0x03,
}drive_state;
Board_init();
Serial_init();
Timer_init();
Drive_init();
int spd = 20;
//Magnetometer_init();
printf("Boat initialized.\n");
Drive_forward(spd);
Timer_new(TIMER_TEST,TEST_DELAY);
drive_state = forward;
int flag = 0;
while (1) {
switch(drive_state){
case forward:
Drive_forward(spd);
if(Timer_isExpired(TIMER_TEST)){
drive_state = idle;
Drive_stop();
Timer_new(TIMER_TEST,TEST_DELAY);
flag = 0;
}
break;
case idle:
Drive_stop();
if(Timer_isExpired(TIMER_TEST)){
if (flag == 0){
drive_state = backward;
Drive_backward(spd);
}else if (flag == 1){
drive_state = forward;
Drive_forward(spd);
}
Timer_new(TIMER_TEST,TEST_DELAY);
}
break;
case backward:
Drive_backward(spd);
if(Timer_isExpired(TIMER_TEST)){
drive_state = idle;
Drive_stop();
Timer_new(TIMER_TEST,TEST_DELAY);
flag = 1;
}
}
Drive_runSM();
}
Drive_stop();
Drive_runSM();
return SUCCESS;
}
