// Test the motor driver
void testMotorDriver(void)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 0);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, 0);
while (1)
{
WDT_Reset();
int i;
for (i = 0; i < 20; i++)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 60);
_delay_ms(100);
WDT_Reset();
}
for (i = 0; i < 20; i++)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 0);
_delay_ms(100);
WDT_Reset();
}
for (i = 0; i <= 60; i++)
{
WDT_Reset();
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, i);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, i);
_delay_ms(20);
}
WDT_Reset();
_delay_ms(100);
for (; i >= 0; i--)
{
WDT_Reset();
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, i);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, i);
_delay_ms(20);
}
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
}
}
void naiboard_delay_us(uint16_t us) {
uint16_t i;
for (i = 0; i < us; i++) {
_delay_us(1);
WDT_Reset(); // Watchdog reset
}
}
void Callback_5ms() {
if (sonarGetState() == SONAR_TRIGGER) {
sonarTriggerStop();
sonarEchoCountStart();
WDT_Reset(&sonarWDT);
}
}
void testPropJoy(void)
{
while(1) {
WDT_Reset();
printf("Speed: %d Dir: %d\n", getWiredPropJoySpeed(), getWiredPropJoyDirection());
_delay_ms(200);
}
}
static void eepromCorrupt()
{
motorEStop();
while (1)
{
lcdText("EEPROM corrupt", "Ver. " __DATE__, 0);
WDT_Reset();
}
}
/* main function */
int main(void)
{
/* ============================== */
/* initialization =============== */
/* ============================== */
WDT_EnableAndSetTimeout(WDT_PER_512CLK_gc);
/* set up LED pins */
init_leds();
/* set up I2C as slave */
init_twi();
/* set up our clocks */
init_clock();
/* set up sensors */
init_sensors();
/* set up motors */
init_motors();
/* set up crude digital outputs */
init_digout();
/* Flash all the LEDs for two and a half seconds to make sure
* they're hooked up */
led_orders->behavior = LED_BEHAVIOR_TIMED;
led_orders->time = 4000;
led_error1->behavior = LED_BEHAVIOR_TIMED;
led_error1->time = 6000;
led_error2->behavior = LED_BEHAVIOR_TIMED;
led_error2->time = 8000;
led_mota->behavior = LED_BEHAVIOR_TIMED;
led_mota->time = 10000;
led_motb->behavior = LED_BEHAVIOR_TIMED;
led_motb->time = 12000;
/* motA.duty = 4000; */
/* motA.direction = 1; */
/* motB.duty = 16000; */
/* motB.direction = 1; */
/* enable interrupts - things start ticking now */
sei();
/* ============================== */
/* main loop ==================== */
/* ============================== */
for(;;)
{
_delay_us(10);
WDT_Reset();
}
}
/*******************************************************************************
* Function name: main
* Description : Main program function. Initializes the peripherals used in the
* demo and executes a loop that reads the ADC and updates the
* display once for each press of switch 1.
* Arguments : none
* Return value : none
*******************************************************************************/
void main(void) {
/* One time initialize instructions */
Setup();
/* Endless loop*/
while (1) {
if (timers.timer_1mS) {
timers.timer_1mS = 0;
Callback_1ms(); //Operations to do every 1ms
if (timers.timer_5mS) {
timers.timer_5mS = 0;
Callback_5ms(); // Operations to do every 5ms
if (timers.timer_10mS) {
timers.timer_10mS = 0;
Callback_10ms(); // Operations to do every 10ms
if (timers.timer_20mS) {
timers.timer_20mS = 0;
Callback_20ms(); // Operations to do every 20ms
}
if (timers.timer_50mS) {
timers.timer_50mS = 0;
Callback_50ms();
if (timers.timer_100mS) {
timers.timer_100mS = 0;
Callback_100ms();
if (timers.timer_500mS) {
timers.timer_500mS = 0;
Callback_500ms();
if (timers.timer_1000mS) {
timers.timer_1000mS = 0;
Callback_1000ms();
}
}
}
}
}
}
}
WDT_Reset(&mainWDT);
// //result = map(analogRead, 0, 4095, 1000, 2200);
// Motor_Write_us(MOTOR_UPPER, desiredState.key.avg_motor_us + desiredState.key.motor_diff_us);
// Motor_Write_us(MOTOR_BOTTOM, desiredState.key.avg_motor_us - desiredState.key.motor_diff_us);
//
// //result = map(analogRead, 0, 4095, 60, 120);
// Servo_Write_deg(1, desiredState.key.x_servo_deg + 18); //18° is the trim of the servo
// Servo_Write_deg(2, desiredState.key.y_servo_deg);
}
Fallback();
} /* End function main() */
void testJoystickDriveMotors(void)
{
//range = 1150
//zero = 2720
int16_t speed;
int16_t dir;
while(1) {
WDT_Reset();
speed = getWiredPropJoySpeed();
dir = getWiredPropJoyDirection();
speed = (speed - 2715) / 4;
dir = (dir - 2725) / 4;
//printf("Speed: %d Dir: %d\n", speed, dir);
if (speed >= 0) {
if (speed > 127)
speed = 127;
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MIXED_MODE, speed);
} else {
speed = -speed;
if (speed > 127)
speed = 127;
sendMotorCommand(MOTOR_CMD_DRIVE_BACKWARDS_MIXED_MODE, speed);
}
if (dir >= 0) {
if (dir > 127)
dir = 127;
sendMotorCommand(MOTOR_CMD_TURN_RIGHT_MIXED_MODE, dir);
} else {
dir = -dir;
if (dir > 127)
dir = 127;
sendMotorCommand(MOTOR_CMD_TURN_LEFT_MIXED_MODE, dir);
}
}
}
/**
* Callback function that will reset the WDT
* @param xTimer
*/
extern "C" void watchdogTimerCallback( xTimerHandle xTimer ) {
WDT_Reset();
}
