void main(int argc, char**argv){
if(argc != 2){
fprintf(stderr,"Error: usage: ./calibrate <file_name>\n");
exit(EXIT_FAILURE);
}
// Init
printf("init\n");
init_gui();
init_motors();
init_sensor();
printf("+---+---+\n");
printf("| 2 | 3 |\n");
printf("+---+---+\n");
printf("| 0 | 1 |\n");
printf("+---+---+\n");
printf(" ^ \n");
printf(" | \n");
printf("Init compass\n");
init_compass();
printf("Calibrate compass\n");
calibrate_location(argv[1]);
destroy_sensors();
destroy_motors();
}
void setup(void) {
init_messages();
init_motors();
init_inertial_sensors();
init_control();
initTime();
}
int main()
{
init_ATX_power();
init_limit_switches();
init_timer();
init_motors();
sei();
enable_ATX_power();
delay_milliseconds(100);
enable_x_motor();
delay_milliseconds(200);
#define CONTINUOUS_HOMING 0
#if CONTINUOUS_HOMING
while (1) {
home_x();
delay_milliseconds(4000);
move_x(MM_to_uSTEPS(MOVE_DISTANCE));
delay_milliseconds(200);
}
#else
home_x();
delay_milliseconds(500);
disable_x_motor();
disable_ATX_power();
while (1)
continue;
#endif
}
void main()
{
light_environnement();
init_motors();
_motor_initial_speed_ = 50;
_mode_light_ = GO_LIGHT;
go_light_touch();
}
void main()
{
init_motors();
_mode_light_ = GO_LIGHT;
light_environnement();
_motor_initial_speed_ = 50;
go_light_stop(20);
}
/* 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();
}
}
task main()
{
init_motors();
while(1) {
motor[mlshooter] = motor[mrshooter] = 127;
motor[mintake1] = motor[mintake2] = motor[mintake3] = 127;
}
}
int main()
{
init_motors();
init_peripherals();
// find out how many cells are connected
u16 batteryCutoff;
while((batteryCutoff = getBatteryVoltage()) == 0);
if(batteryCutoff > 720) // voltage is higher than 6V -> 2cells
batteryCutoff = 720; // cutoff 6V
else
batteryCutoff = 360; // cutoff 3V
calibrate_sensors();
// enable interrupt driven gyro acquisation
startGyro();
u08 c = 0, maxThrust = (batteryCutoff>360)?70:100;
u08 cutBattery = 0;
while(1) {
if(Thrust > maxThrust)
Thrust = maxThrust;
calculate_balance(Thrust, Yaw, Pitch, Roll);
checkForInput();
// check battery
if(((c%100)==0)) {
u16 bat = getBatteryVoltage();
if(bat > 0 && bat <= batteryCutoff) {
if(maxThrust > 0)
maxThrust -= 5;
cutBattery = 1;
}
}
if(!cutBattery || !(c % 50))
PORTC ^= (1<<2);
c++;
//_delay_ms(100);
}
}
void perform_inits(void)
{
g_defined_temp = 35;
g_current_temp = 0.;
init_usart(BAUDRATE, TRANSMIT_RATE, DATA_BITS, STOP_BITS, PARITY_BITS);
init_peltier_port();
init_motors();
init_motors_timer();
init_lcd();
sei();
}
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
BCSCTL1 = CALBC1_8MHZ; // 8Mhz calibration for clock
DCOCTL = CALDCO_8MHZ;
init_sensors();
init_wdt();
init_motors();
init_lastData();
_bis_SR_register(GIE+LPM0_bits); //enable general interrupts and power down CPU
}
void init(int use_http, char *address, int port){
// Setup periferal and library
init_gui();
init_motors();
init_sensor();
init_compass();
calibrate_compass(0); // 0 = do not execute a new calibration of the compass
// Load ball predefined positions
get_positions(POINTS_FILE_NAME, &positions, &n_points);
// Start bluetooth
if(use_http)
bluetooth_test_init(address, port);
else
bluetooth_init();
//bluetooth_register_and_start(fAction_t, fAck_t, fLead_t, fStart_t, fStop_t, fWait_t, fKick_t, fCancel_t);
bluetooth_register_and_start(&follower_action_cb, NULL, &lead_cb, &start_cb, &stop_cb, NULL, NULL, &cancel_cb);
// Reset global variables
send_action_flag = 1;
act = 0;
end_game = 0;
start_game = 0;
wait = 0;
cancel = 0;
// Waiting for the game start
set_light(LIT_LEFT, LIT_GREEN);
set_light(LIT_RIGHT, LIT_GREEN);
while(!start_game); //start game, robot rank and snake size initialized by start_cb
// Initial position
home.x = BORDER_X_MAX/2;
home.y = (snake_size - robot_rank -1)*40 + 20;
center.x = BORDER_X_MAX/4;
center.y = BORDER_Y_MAX/2;
robot = home;
printf("[DEBUG] Starting position X: %d, Y: %d, T: %d\n", robot.x , robot.y, robot.t);
update_sensor(SENSOR_GYRO);
gyro_init_val = get_sensor_value(SENSOR_GYRO);
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
}
int main(int argc,char** argv){
int mot_value[] = {200, 400, 600, 1000, 1200};
int us_value[NUM_TESTS];
int i;
// Init
init_gui();
init_motors();
init_sensor();
// US setup
set_motors_speed(NORMAL_SPEED);
printf("mot\tus\n");
for(i=0; i<NUM_TESTS; i++){
us_value[i] = do_test(mot_value[i]);
}
return 0;
}
void application_main(void)
{
unsigned char toto[2][4]={{0x05,0x01,0x00,0xf0}, // 00000101, 00000001, 00000000, 11110000
{0x0A,0x02,0x00,0xf0}};// 00001010, 00000010, 00000000, 11110000
// Reset the Timer0 value
TMR0H = 0;
TMR0L = 1;
setalim();
init_motors();
setup_adconversion();
init_servos();
init_digitals_in();
init_debug();
init_fdc();
set_fdc((unsigned char *)toto,2);
SET_DEVICE_STATUS(REMOTE_WAKEUP_DIS);
// Interruptions
PIE2bits.USBIE = 0; // Interrupt USB off : on ne touche pas a la gestion de l'USB !!
INTCONbits.PEIE = 1; // Interrupts peripheriques (usb, timer 1...)
INTCONbits.GIE = 1; // Interrupts global
while(usb_active_cfg > 2)
{
usb_sleep();
dispatch_usb_event();
}
cutalim();
PINDEBUG = 0;
verif_fdc(0);
// clear_device_remote_wakeup();
}
/**
* Move forward, and when a wall is detected, follow it
* Last update: -
* @version -
*/
void main()
{
int ir_current; /* Store, temporarily the ir values */
int ir_previous = 0;
int light;
/* Initialize all components needed to run the robot */
init_motors();
_motor_initial_speed_ = 100;
start_process(running_forever());
while(1)
{
int enc_dif = _right_enc_counts_-_left_enc_counts_;
/*
* Depending on the ir_detect() value),
* make it rotate, and move forward again
*/
ir_current = ir_detect();
/* Something is detected */
if(ir_current != 0)
{
if(ir_current == OBSTACLE_FRONT)
{
printf("FRONT\n");
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
/* Check the light difference */
light = light_diff();
rotate(C_MOTOR, ((light < 0) - (light >= 0))*90);
/* The robot can go foward after the dodge */
start_process(running_forever());
ir_previous = 0;
}
else
{
printf("move out %d\n", 10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
rotate(C_MOTOR, 10 * ((ir_current == OBSTACLE_LEFT) - (ir_current == OBSTACLE_RIGHT)));
/* The robot can go foward after the dodge */
start_process(running_forever());
ir_previous = ir_current;
}
}
else if(ir_previous)
{
printf("move to %d\n", -10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
/* Check the light difference */
rotate(C_MOTOR, -10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* The robot can go foward after the dodge */
start_process(running_forever());
}
sleep(0.1);
}
}
void startSearch()
{
int currentNodeID = 0; //Current node while starting is 0
init_Dfs(); //Initialize the stack spine
init_motors();
run();
int nextNodeID;
addNeigbourNodesToFrontier(currentNodeID);
addToSet(&markedNodes,currentNodeID);
push(&spine, currentNodeID);
nextNodeID = DecideNextNode(currentNodeID);
int cnt = 0;
while(1)
{
//scanAndSearch(); //openCV code
if(objectFound == true)
{
// Functionality here TBD
printf("Obj found: %d",cnt);
cnt ++;
rotate360();
objectFound = false;
/* while(isObjectClose() == 0)
moveForward(); // Provided by the motor library
if(distanceTravelledFromPrevNode == distanceToNextNode)
addToSet(&markedNodes,currentNodeID);
//?? Who updates the current position of the object
*/
//Keep track of the distance so that obj position is known
//Keep track of where we are in the graph
}
else
{
printf("Spine. \n");
printStack(&spine);
printf("\n\n");
printf("Next node to be traversed: %d\n", nextNodeID);
moveToNextNode(currentNodeID,nextNodeID); //Move to the next node
currentNodeID = nextNodeID;
addToSet(&markedNodes,currentNodeID); //Current Node has been traversed. Add to the set
addNeigbourNodesToFrontier(currentNodeID); //Adding frontier nodes from the new current node
removeNodeFromSet(&frontierNodes,currentNodeID); //Removing the current node from frontier
printf("Done nodes : \n");
listSet(markedNodes);
printf("Frontier Nodes : \n");
listSet(frontierNodes);
if (checkIfNeighborExistsInFrontier(currentNodeID)) //Check if neighbor exists in frontier
{ // If so visit there
push(&spine, currentNodeID); //Change
nextNodeID = DecideNextNode(currentNodeID);
}
else
{
if(isStackEmpty(&spine))
return ; // Done traversing. Stop
else
{
nextNodeID = pop(&spine);
}
}
}
}
}
int main(void) {
delay_ms(100);
init_leds();
init_timers();
init_motors();
// Used to print to serial comm window
char tempBuffer[80];
// Initialization here.
lcd_init_printf(); // required if we want to use printf() for LCD printing
init_menu(); // this is initialization of serial comm through USB
clear(); // clear the LCD
//enable interrupts
sei();
while (1) {
process_Trajectory();
if (updateLCD)
{
//first line
lcd_goto_xy(8,0);
print(" ");
lcd_goto_xy(8,0);
//print_long(global_counts_m1);
sprintf( tempBuffer, "V=%d\r", voltage);
print(tempBuffer);
//next line
lcd_goto_xy(0,1);
sprintf( tempBuffer, "R=%3.2f RPM=%3.2f\r", rotations, RPM);
print(tempBuffer);
updateLCD = false;
}
if (display_values_flag)
{
if (TARGET_ROT)
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:%f T:%d\r\n", Kp, Ki, Kd, goal, rotations, voltage);
else if (TARGET_RPM)
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:%f T:%d\r\n", Kp, Ki, Kd, goal, RPM, voltage);
else
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:NA T:%d\r\n", Kp, Ki, Kd, goal, voltage);
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "C0:%d C1:%d C2:%d C3:%d C4:%d C5:%d Man:%d Eq:%d\r\n", command_list[0],
command_list[1],
command_list[2],
command_list[3],
command_list[4],
command_list[5],
maneuver_complete,
equilibrium);
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "%f, %f, %f\r\n", error, integral, derivative);
print_usb( tempBuffer, length );
display_values_flag = false;
}
if (ready_to_dump_log)
{
ready_to_dump_log = false;
length = sprintf( tempBuffer, "Goal, Kp, Ki, Kd, Rot, RPM, Volt\r\n");
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "%f, %f, %f, %f, %f, %f, %d\r\n", goal, Kp, Ki, Kd, rotations, RPM, voltage);
print_usb( tempBuffer, length );
int delta = (log_stop_timestep-log_start_timestep);
length = sprintf( tempBuffer, "Time, Error, KpError, KiIntegral, KdDerivative, Rotations, RPM, Voltage\r\n");
print_usb( tempBuffer, length );
float temp_Kp = (Kp == 0.0) ? 1.0 : Kp;
float temp_Ki = (Ki == 0.0) ? 1.0 : Ki;
float temp_Kd = (Kd == 0.0) ? 1.0 : Kd;
float time;
for(int i=0; i<delta; i++)
{
time = (float)((i*dt)*60);
length = sprintf( tempBuffer, "%f %f %f %f %f %f %f %d\r\n", time, //time expressed in seconds
LOG[i].error,
(float)(temp_Kp * LOG[i].error),
(float)(temp_Ki * LOG[i].integral),
(float)(temp_Kd * LOG[i].derivative),
LOG[i].rotations,
LOG[i].RPM,
LOG[i].voltage);
print_usb( tempBuffer, length );
}
}
//Process Control from the USB Port
serial_check();
check_for_new_bytes_received();
} //end while loop
} //end main
