static void app_rover_setup(app_common *com, app_remote *rem, configuration_t *cnf) {
common = com;
remote = rem;
app_cfg = cnf;
#ifdef CONFIG_SPYBOT_HCSR
RANGE_SENS_init(app_rover_radar_cb);
#endif
#ifdef CONFIG_I2C
I2C_config(_I2C_BUS(0), 100000);
STMPE_init();
#ifdef CONFIG_SPYBOT_LSM
task *config_lsm_t = TASK_create(app_rover_setup_lsm, 0);
ASSERT(config_lsm_t);
TASK_run(config_lsm_t, 0, NULL);
#endif
CFG_EE_init(&eeprom_dev, app_rover_cfg_cb);
CFG_EE_load_config();
#endif // CONFIG_I2C
#ifdef CONFIG_SPYBOT_MOTOR
MOTOR_init();
#endif
#ifdef CONFIG_SPYBOT_SERVO
SERVO_init();
#endif
mech_task = TASK_create(app_rover_mech_task, TASK_STATIC);
TASK_start_timer(mech_task, &mech_timer, 0, 0, 0, 20, "mech");
#ifdef CONFIG_SPYBOT_HCSR
radar_task = TASK_create(app_rover_radar_task, TASK_STATIC);
TASK_start_timer(radar_task, &radar_timer, 0, 0, 100, 65, "radar");
#endif // CONFIG_SPYBOT_HCSR
#ifdef CONFIG_SPYBOT_LSM
int i;
for (i = 0; i < 3; i++) {
acc_extremes[i][0] = S16_MAX;
acc_extremes[i][1] = S16_MIN;
mag_extremes[i][0] = S16_MAX;
mag_extremes[i][1] = S16_MIN;
}
#endif
COMRAD_init();
}
int main (void){
float acc_x, acc_z, gyro_x;
float acc_angle,kal_angle;
/* Init Systick to 1ms */
SysTick_Config( SystemCoreClock / 1000);
/* Initialize GPIO (sets up clock) */
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);
SERVO_init();
if(I2CInit(I2CMASTER) == FALSE){
while(1); /* fatal error */
}
if(MPU6050_init()){
return 0;
}
MPU6050_setZero();
kalman_init();
for(;;){
/* 100Hz loop */
if(gSysTick_10 >= 9){
gSysTick_10 = 0;
/* get sensor values */
gyro_x = MPU6050_getGyroRoll_degree();
acc_x = MPU6050_getAccel_x();
acc_z = -MPU6050_getAccel_z();
/* acc angle */
acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle
kal_angle = kalman_update(acc_angle,gyro_x, 0.01);
SERVO_set_slew((-kal_angle) - MECH_OFFSET);
}
}
}
void mainInit()
{
USART_Init(MYUBRR);
fdevopen(USART_Transmit, USART_Receive);
SPI_Init();
CAN_init();
SERVO_init();
TWI_Master_Initialise();
SOLENOID_init();
MOTOR_init();
IR_init();
sei();
}
int main(void)
{
DDRC |= (1<<PORTC6) | (1<<PORTC7); //init LEDs
//servoTx;
SERVO_init(); //Init servos
USART_init();
init_counters();
set_counter_1(10000);
initvar();
sei();
SERVO_update_EEPROM(BROADCASTING_ID);
//move_to_std();
// ------ TESTCODE FOR READING SERVO -------
//servoGoto(1, 3.14/3, 0x200);
SERVO_update_EEPROM(BROADCASTING_ID); // NOTE: needs to run once for SERVO_get position to work
//----------------------------
_delay_ms(3000);
reset_counter_1();
set_counter_1(3000);
while(1)
{
/*
uint8_t r = USART_getRotation();
uint8_t s = USART_getSpeed();
uint8_t d = USART_getDirection();
if(s != 0 || r != 50)
{
std_pos_flag = 0;
reset_counter_1();
}
move_robot(d, r, s);
if(r == 50 && s == 0 && d == 0)
{
_delay_ms(50);
//PORTD ^= (1<<PORTD5);
cli();
USART_send_ready();
sei();
}
*/
/*change_z(-130);
move_to_std();
for(int i = 0; i < 5; ++i)
{
move_robot(0,50,100);
//_delay_ms(2000);
}
_delay_ms(1000);
move_to_std();
_delay_ms(1000);
*/
climb();
//climb_all_one_leg();
//SERVO_update_data(12);
//USART_SendValue(SERVO_get_load());
USART_DecodeRxFIFO();
_delay_ms(200);
}
}
int main(void)
{
LED_INIT;
//servoTx;
sei();
USART_init();
MPU_init();
SERVO_init();
init_counters();
initvar();
SERVO_update_EEPROM(BROADCASTING_ID);
wait(10);
move_to_std();
wait_until_gyro_stable();
LED0_ON;
USART_send_message("Gyro Stable");
// ------ TESTCODE FOR READING SERVO -------
//servoGoto(1, 3.14/3, 0x200);
SERVO_update_EEPROM(BROADCASTING_ID); // NOTE: needs to run once for SERVO_get position to work
//----------------------------
reset_counter_1();
set_counter_1(3000);
uint8_t readyCounter = 0;
while(1)
{
MPU_update();
if(USART_get_turn_flag())
{
turn_degrees(USART_get_turn_angle(), USART_get_turn_dir());
}
if(USART_get_climb_flag())
{
climb();
}
uint8_t r = USART_getRotation();
uint8_t s = USART_getSpeed();
uint8_t d = USART_getDirection();
if(s != 0 || r != 50)
{
std_pos_flag = 0;
reset_counter_1();
readyCounter = 3;
}
move_robot(d, r, s);
if(r == 50 && s == 0 && d == 0 && readyCounter)
{
cli();
USART_send_ready();
sei();
readyCounter--;
}
if(move_to_std_flag == 1)
{
move_to_std_flag = 0;
move_to_std();
}
/*climb();
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
/*
change_z(-120);
move_to_std();
turn_degrees(180,1);
// Takes a predecided number of steps forward
// This is good when testing different things.
wait(100);
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
USART_decode_rx_fifo();
}
}
