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(){
//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){
;
}
#endif
printf("Actuator Test Harness Initiated\n\n");
//Test Rudder
printf("Centering rudder.\n");
setRudder(RUDDER_TURN_LEFT, 0);
delayMillisecond(ACTUATOR_DELAY);
printf("Turning rudder left.\n");
setRudder(RUDDER_TURN_LEFT, 100); //push to one direction
delayMillisecond(ACTUATOR_DELAY);
printf("Centering rudder.\n");
setRudder(RUDDER_TURN_LEFT, 0);
delayMillisecond(ACTUATOR_DELAY);
printf("Turning rudder right.\n");
setRudder(RUDDER_TURN_RIGHT, 100);
delayMillisecond(ACTUATOR_DELAY);
printf("Centering rudder.\n");
setRudder(RUDDER_TURN_LEFT, 0);
//Test Motor Left
printf("Testing left motor.\n");
setLeftMotor(0);
delayMillisecond(ACTUATOR_DELAY);
printf("Driving left motor forward.\n");
setLeftMotor(100);
delayMillisecond(ACTUATOR_DELAY);
setLeftMotor(0);
delayMillisecond(ACTUATOR_DELAY);
//printf("Driving left motor reverse.\n");
//setLeftMotor(MIN_PULSE);
//delayMillisecond(ACTUATOR_DELAY);
//setLeftMotor(0);
//Test Motor Right
printf("Testing right motor.\n");
setRightMotor(0);
delayMillisecond(ACTUATOR_DELAY);
printf("Driving right motor forward.\n");
setRightMotor(100);
delayMillisecond(ACTUATOR_DELAY);
setRightMotor(0);
delayMillisecond(ACTUATOR_DELAY);
//printf("Driving right motor reverse.\n");
//setRightMotor(MIN_PULSE);
//delayMillisecond(ACTUATOR_DELAY);
//setRightMotor(STOP_PULSE);
//delayMillisecond(ACTUATOR_DELAY);
// // Remove this code
// setRightMotor(MAX_PULSE);
// setLeftMotor(MAX_PULSE);
// while (1)
// asm("nop");
//
//
// printf("\nDone with drive test.\n");
return SUCCESS;
}
/**********************************************************************
* Function: updateHeading
* @return None
* @remark Determines the heading error using the tilt-compensated compass,
* and adjusts the rudder accordingly. Also, a bang-bang control has been
* implemented to turn the rudder to the maximum value if the boat's motors
* are being driven below some percentage defined above.
* @author Darrel Deo
* @author David Goodman
* @date 2013.03.27
**********************************************************************/
static void updateRudder() {
static int16_t lastThetaError; // used for derivative term
rudderDirection = RUDDER_TURN_LEFT;
// Get current heading, determine theta error
uint16_t currentHeading = (uint16_t)TiltCompass_getHeading();
int16_t thetaError = desiredHeading - currentHeading;
// Bound theta error and determine turn direction
if (thetaError > 0) {
rudderDirection = (thetaError < 180)? RUDDER_TURN_RIGHT : RUDDER_TURN_LEFT;
thetaError = (thetaError < 180)? thetaError : (360 - thetaError);
}
else {
// theta error is negative
rudderDirection = (thetaError > -180)? RUDDER_TURN_LEFT : RUDDER_TURN_RIGHT;
thetaError = (thetaError > -180)? -thetaError : (360 + thetaError);
}
// Initialize or dump derivative if changed directions
if (lastRudderDirection == RUDDER_TURN_NONE || rudderDirection != lastRudderDirection)
lastThetaError = 0;
/* Controller Terms */
// Derivative (scaled by KD_RUDDER)
float thetaErrorDerivative = (float)abs(thetaError -
lastThetaError)/MS_TO_SEC(TRACK_UPDATE_DELAY);
// Proportional (scaled by KP_RUDDER), convert degrees to percent
float uDegrees = KP_RUDDER*(float)thetaError + KD_RUDDER*thetaErrorDerivative;
float uPercent = (uDegrees / RUDDER_ANGLE_MAX ) * 100;
// Limit percentage from 0 to 100
uPercent = (uPercent > 100.0)? 100.0f : uPercent;
uPercent = (uPercent < 0.0)? 0.0f : uPercent;
char *bangbang = "";
// Bang-bang control to force rudder all the way if speed is low
if (desiredSpeed < RUDDER_BANGBANG_SPEED_THRESHOLD
&& thetaError > RUDDER_BANGBANG_THETA_DEADBAND_THRESHOLD) {
uPercent = 100.0f;
bangbang = " BB";
}
// Command the rudder and save
setRudder(rudderDirection, (uint8_t)uPercent);
lastThetaError = thetaError;
lastRudderDirection = rudderDirection;
char *dir;
dir = (rudderDirection == RUDDER_TURN_RIGHT)? "R" : "L";
#ifdef DEBUG_VERBOSE
DBPRINT("Rudder control: rDegrees=%d, yDegrees=%d, eDegrees=%d, uDegrees=%.2f, uPercent=%d[%s]\n\n",
desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir);
#endif
#ifdef USE_PUBLIC_DEBUG
sprintf(debugString, "R=%d, Y=%d, e=%d, U=%.2f, Up=%d[%s]%s, S=%d\n",
desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir, bangbang, desiredSpeed);
#endif
}
CX10::CX10() {
randomSeed((analogRead(A0) & 0x1F) | (analogRead(A1) << 5));
for (int n = 0; n < CRAFT; ++n) {
Craft *c = &craft_[n];
c->nextPacket = 0;
memset(c->aid, 0xff, sizeof c->aid);
memset(c->Servo_data, 0, sizeof c->Servo_data);
setThrottle(n, 0);
setAileron(n, 500);
setElevator(n, 500);
setRudder(n, 500);
for(uint8_t i=0;i<4;i++) {
#ifdef RFDUINO
c->txid[i] = random(256);
#else
c->txid[i] = random();
#endif
}
c->txid[1] %= 0x30;
c->freq[0] = (c->txid[0] & 0x0F) + 0x03;
c->freq[1] = (c->txid[0] >> 4) + 0x16;
c->freq[2] = (c->txid[1] & 0x0F) + 0x2D;
c->freq[3] = (c->txid[1] >> 4) + 0x40;
}
pinMode(ledPin, OUTPUT);
//RF module pins
pinMode(MOSI_pin, OUTPUT);
pinMode(SCK_pin, OUTPUT);
pinMode(MISO_pin, INPUT);
pinMode(CS_pin, OUTPUT);
pinMode(CE_pin, OUTPUT);
digitalWrite(ledPin, LOW);//start LED off
CS_on;//start CS high
CE_on;//start CE high
MOSI_on;//start MOSI high
SCK_on;//start sck high
delay(70);//wait 70ms
CS_off;//start CS low
CE_off;//start CE low
MOSI_off;//start MOSI low
SCK_off;//start sck low
delay(100);
CS_on;//start CS high
delay(10);
#ifdef RFDUINO
SPI.begin();
SPI.setFrequency(250);
#endif
//############ INIT1 ##############
CS_off;
_spi_write(0x3f); // Set Baseband parameters (debug registers) - BB_CAL
_spi_write(0x4c);
_spi_write(0x84);
_spi_write(0x67);
_spi_write(0x9c);
_spi_write(0x20);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x3e); // Set RF parameters (debug registers) - RF_CAL
_spi_write(0xc9);
_spi_write(0x9a);
_spi_write(0xb0);
_spi_write(0x61);
_spi_write(0xbb);
_spi_write(0xab);
_spi_write(0x9c);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x39); // Set Demodulator parameters (debug registers) - DEMOD_CAL
_spi_write(0x0b);
_spi_write(0xdf);
_spi_write(0xc4);
_spi_write(0xa7);
_spi_write(0x03);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x30); // Set TX address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x2a); // Set RX pipe 0 address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
_spi_write_address(0xe1, 0x00); // Clear TX buffer
_spi_write_address(0xe2, 0x00); // Clear RX buffer
_spi_write_address(0x27, 0x70); // Clear interrupts
_spi_write_address(0x21, 0x00); // No auto-acknowledge
_spi_write_address(0x22, 0x01); // Enable only data pipe 0
_spi_write_address(0x23, 0x03); // Set 5 byte rx/tx address field width
_spi_write_address(0x25, 0x02); // Set channel frequency
_spi_write_address(0x24, 0x00); // No auto-retransmit
_spi_write_address(0x31, PACKET_LENGTH); // 19-byte payload
_spi_write_address(0x26, 0x07); // 1 Mbps air data rate, 5dbm RF power
_spi_write_address(0x50, 0x73); // Activate extra feature register
_spi_write_address(0x3c, 0x00); // Disable dynamic payload length
_spi_write_address(0x3d, 0x00); // Extra features all off
MOSI_off;
delay(100);//100ms delay
//############ INIT2 ##############
healthy_ = _spi_read_address(0x10) == 0xCC;
_spi_write_address(0x20, 0x0e); // Power on, TX mode, 2 byte CRC
MOSI_off;
delay(100);
bindAllowed_ = true;
nextBind_ = micros();
nextSlot_ = 0;
}