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();
}
Exemple #3
0
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
}
Exemple #4
0
void main()
{
    light_environnement();
    init_motors();
    _motor_initial_speed_ = 50;
    _mode_light_ = GO_LIGHT;
    go_light_touch();
}
Exemple #5
0
void main()
{
    init_motors();
    _mode_light_ = GO_LIGHT;
    light_environnement();
    _motor_initial_speed_ = 50;
    go_light_stop(20);
}
Exemple #6
0
/* 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;
	}
}
Exemple #8
0
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);
	}

}
Exemple #9
0
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();
}
Exemple #10
0
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;
}
Exemple #13
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();
}
Exemple #14
0
/**
 * 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);
    }
}
Exemple #15
0
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