Пример #1
0
void    autonomous_routine2()

{
    shaft_t         shafts[2];
    
    controller_begin_autonomous_mode();

    /* Drive forward for a while */
    shaft_tps_init(&shafts[0], 0, ROUTINE2_TICKS,
	LEFT_DRIVE_PORT, LEFT_ENCODER_INTERRUPT_PORT, 0, -ROUTINE2_POWER);
    shaft_tps_init(&shafts[1], 0, ROUTINE2_TICKS,
	RIGHT_DRIVE_PORT, RIGHT_ENCODER_INTERRUPT_PORT, 0, ROUTINE2_POWER);
    shaft_tps_run(shafts, 2);
    
    pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
    pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
    controller_submit_data(WAIT);
    
    /* Drive backward for a while */
    shaft_tps_init(&shafts[0], 0, ROUTINE2_TICKS,
	LEFT_DRIVE_PORT, LEFT_ENCODER_INTERRUPT_PORT, 0, ROUTINE2_POWER);
    shaft_tps_init(&shafts[1], 0, ROUTINE2_TICKS,
	RIGHT_DRIVE_PORT, RIGHT_ENCODER_INTERRUPT_PORT, 0, -ROUTINE2_POWER);
    shaft_tps_run(shafts, 2);
    
    pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
    pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
    controller_submit_data(WAIT);
    
    controller_end_autonomous_mode();
}
Пример #2
0
void    autonomous_routine3()

{
    int     acceleration,
	    c;
    long    velocity = 0,
	    position = 0;

    controller_begin_autonomous_mode();
    
    pwm_write(LEFT_DRIVE_PORT, -50);
    pwm_write(RIGHT_DRIVE_PORT, 50);
    controller_submit_data(WAIT);
    
    for (c=0; c<100; ++c)
    {
	read_accelerometer_axis(4, 535, 5000, &acceleration, &velocity, &position);
	delay_msec(10);
    }
    
    pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
    pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
    controller_submit_data(WAIT);
    
    controller_end_autonomous_mode();
}
Пример #3
0
void    arcade_drive_routine(void)

{
    char        left_power,
		right_power,
		impeller_power,
		arm_power,
		joy_x,
		joy_y;
    
    joy_x = rc_read_data(ARCADE_DRIVE_X_CHAN);
    joy_y = -rc_read_data(ARCADE_DRIVE_Y_CHAN);
    arcade_drive(joy_x, joy_y, PWM_MAX, &left_power, &right_power);
    // printf("%d %d %d %d\n", joy_x, joy_y, left_power, right_power);

    /* Drive motors */
    pwm_write(LEFT_DRIVE_PORT, -left_power);
    pwm_write(RIGHT_DRIVE_PORT, right_power);
    
    /* Implements */
    impeller_power = rc_read_data(ARCADE_IMPELLER_CHAN);
    arm_power = rc_read_data(ARCADE_ARM_CHAN);
    pwm_write(IMPELLER_PORT, impeller_power);
    pwm_write(ARM_PORT, arm_power);
}
Пример #4
0
void    autonomous_routine0()

{
    int             c;
    shaft_t         shafts[2];
    
    controller_begin_autonomous_mode();

    for (c=0; c<4; ++c)
    {
	/* Drive forward for a while */
	if ( shaft_tps_init(&shafts[0], 0, 180,
	    LEFT_DRIVE_PORT, LEFT_ENCODER_INTERRUPT_PORT, 0, -60) != OV_OK )
	    printf("shaft_tps_init() failed.\n");
	shaft_tps_init(&shafts[1], 0, 180,
	    RIGHT_DRIVE_PORT, RIGHT_ENCODER_INTERRUPT_PORT, 0, 60);
	shaft_tps_run(shafts, 2);
	
	pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
	pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
	controller_submit_data(WAIT);
    
	/* Turn */
	shaft_tps_init(&shafts[0], 0, 105,
	    LEFT_DRIVE_PORT, LEFT_ENCODER_INTERRUPT_PORT, 0, -60);
	shaft_tps_run(shafts, 1);
    }
    
    controller_end_autonomous_mode();
}
Пример #5
0
void    spin1(char power, unsigned short ms)

{
    pwm_write(RIGHT_DRIVE_PORT, +power);
    pwm_write(LEFT_DRIVE_PORT, +power);
    controller_submit_data(WAIT);
    delay_msec(ms);
}
Пример #6
0
void pwm_init(){


	LPC_SYSCON->SYSAHBCLKCTRL |= (1<<SCT); // enable SCT
	LPC_SYSCON->PRESETCTRL |= (1<<SCT_RST_N); // reset SCT

	LPC_SCT->CONFIG = (1<<UNIFY); //SCT_CONFIG_32BIT_COUNTER | SCT_CONFIG_CLKMODE_BUSCLK;

	// Initial CTOUT0 state is high
	LPC_SCT->OUTPUT = (7 << 0);

	cycleTicks = __SYSTEM_CLOCK / PWMCYCLERATE; //1ms ticks

	// Setup for match mode
	LPC_SCT->REGMODE_L = 0;


	// Setup match counter 0 for the number of ticks in a PWM sweep, event 0
	// will be used with the match 0 count to reset the counter.
	LPC_SCT->MATCH[0].U = cycleTicks;
	LPC_SCT->MATCHREL[0].U = cycleTicks;
	LPC_SCT->EVENT[0].CTRL = 0x00001000;
	LPC_SCT->EVENT[0].STATE = 0xFFFFFFFF;
	LPC_SCT->LIMIT_L = (1 << 0);

	// For CTOUT0 to CTOUT2, event 1 is used to clear the output
	LPC_SCT->OUT[0].CLR = (1 << 0);
	LPC_SCT->OUT[1].CLR = (1 << 0);
	LPC_SCT->OUT[2].CLR = (1 << 0);

	//init to 50%
	pwm_write(0, 50);
	pwm_write(1, 50);
	pwm_write(2, 50);

	// Setup event 1 to trigger on match 1 and set CTOUT0 high
	LPC_SCT->EVENT[1].CTRL = (1 << 0) | (1 << 12);
	LPC_SCT->EVENT[1].STATE = 1;
	LPC_SCT->OUT[0].SET = (1 << 1);

	// Setup event 2 trigger on match 2 and set CTOUT1 high
	LPC_SCT->EVENT[2].CTRL = (2 << 0) | (1 << 12);
	LPC_SCT->EVENT[2].STATE = 1;
	LPC_SCT->OUT[1].SET = (1 << 2);

	// Setup event 3 trigger on match 3 and set CTOUT2 high
	LPC_SCT->EVENT[3].CTRL = (3 << 0) | (1 << 12);
	LPC_SCT->EVENT[3].STATE = 1;
	LPC_SCT->OUT[2].SET = (1 << 3);


	// Don't use states
	LPC_SCT->STATE_L = 0;

	// Unhalt the counter to start
	LPC_SCT->CTRL_U &= ~(1 << 2);
}
Пример #7
0
void    arcade_drive_routine(void)

{
    static char rc_pos,
		implement_pos = SERVO_CENTER;
    static char left_drive,
		right_drive,
		joy_x,
		joy_y;
    
    joy_x = rc_read_data(ARCADE_DRIVE_X_CHAN);
    joy_y = -rc_read_data(ARCADE_DRIVE_Y_CHAN);
    arcade_drive(joy_x, joy_y, PWM_MAX, &left_drive, &right_drive);
    // printf("%d %d %d %d\n", joy_x, joy_y, left_drive, right_drive);

    /*
     *  This code is for a forklift implement mounted on a servo.
     *  If forklift is below level, digitally "gear down" the drive motors
     *  for finer control of the robot while trying to slide forks
     *  under a pallet.
     */
    if ( implement_pos > SERVO_CENTER )
    {
	left_drive /= IMPLEMENT_GEAR_DOWN;
	right_drive /= IMPLEMENT_GEAR_DOWN;
    } 
    
    /* Drive motors */
    pwm_write(LEFT_DRIVE_PORT, -left_drive);
    pwm_write(RIGHT_DRIVE_PORT, right_drive);
    
    /*
     *  Servo controlling implement on port 4
     *  If joystick is off center by the
     *  threshold value (to avoid moving due to joystik trim issues and
     *  inadvertent lateral movements) and the servo
     *  position is within bounds, move servo one position up or down.
     */
    rc_pos = -rc_read_data(ARCADE_IMPLEMENT_CHAN);
    if ( (rc_pos < JOY_CENTER - JOY_THRESHOLD) &&
	 (implement_pos < IMPLEMENT_DOWN) )
	++implement_pos;
    else if ( (rc_pos > JOY_CENTER + JOY_THRESHOLD) &&
	      (implement_pos > IMPLEMENT_UP) )
	--implement_pos;
    pwm_write(IMPLEMENT_SERVO_PORT, implement_pos);
}
Пример #8
0
static ssize_t write_rgb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
    const void *buf, uint16_t len, uint16_t offset)
{
    memcpy(rgb, buf, sizeof(rgb));

    printk("write_rgb: %x %x %x\n", rgb[0], rgb[1], rgb[2]);

    pwm_write();

    return sizeof(rgb);
}
Пример #9
0
void    autonomous_routine0(void)

{
    DPRINTF("Starting autonomous routine...\n");
    controller_begin_autonomous_mode();
    
    /*
     *  Place your autonomous code here.  The example below drives
     *  the robot forward (or backward) for 2 seconds and then stops.
     */
    
    pwm_write(RIGHT_DRIVE_PORT, AUTON_DRIVE_SPEED);
    pwm_write(LEFT_DRIVE_PORT, -AUTON_DRIVE_SPEED);
    controller_submit_data(WAIT);
    delay_sec(2);
    pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
    pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
    controller_submit_data(WAIT);
    
    controller_end_autonomous_mode();
    DPRINTF("Ending autonomous routine...\n");
}
Пример #10
0
void    tank_drive_routine(void)

{
    static char left_power,
		right_power,
		impeller_power,
		arm_power;
    
    /* Drive motors face opposite directions, so reverse one side. */
    left_power = rc_read_data(TANK_DRIVE_LEFT_CHAN);
    right_power = -rc_read_data(TANK_DRIVE_RIGHT_CHAN);
    
    /* Drive motors */
    pwm_write(RIGHT_DRIVE_PORT, right_power);
    pwm_write(LEFT_DRIVE_PORT, left_power);
    
    /* Implements */
    impeller_power = rc_read_data(TANK_IMPELLER_CHAN);
    arm_power = rc_read_data(TANK_ARM_CHAN);
    pwm_write(IMPELLER_PORT, impeller_power);
    pwm_write(ARM_PORT, arm_power);
}
Пример #11
0
void    sonar_scan(unsigned char port)

{
    static char sonar_direction = SERVO_CENTER;
    static char add = 2;
    
    /* Servo oscillating sonar direction */
    pwm_write(port, sonar_direction);
    sonar_direction += add;
    
    /* Reverse when position reaches limit */
    if ( (sonar_direction == -SONAR_MAX_ANGLE) ||
	 (sonar_direction == +SONAR_MAX_ANGLE) )
	add = -add;
    controller_submit_data(NO_WAIT);
}
Пример #12
0
void service_init(struct bt_conn *conn)
{
    if ((pwm = device_get_binding("PWM_0")) == NULL)
        printk("device_get_binding: failed for PWM\n");
    else {
        pwm_write();
        printk("PWM init OK\n");
    }

    struct device *ipm;

    ipm = device_get_binding("temperature_ipm");
    ipm_register_callback(ipm, temperature_ipm_callback, conn);
    ipm_set_enabled(ipm, 1);

    bt_gatt_register(attrs, ARRAY_SIZE(attrs));
}
Пример #13
0
void set_backlight(uint32_t brightness)
{
	int index = PWM_INDEX;

	__raw_bits_or((0x1 << 0), REG_AON_CLK_PWM0_CFG + index * 4);//ext_26m select

	if (0 == brightness) {
		pwm_write(index, 0, PWM_PRESCALE);
		printf("sprd backlight power off. pwm_index=%d  brightness=%d\n", index, brightness);
	} else {
		__raw_bits_or((0x1 << (index+4)), REG_AON_APB_APB_EB0); //PWMx EN

		pwm_write(index, PWM2_SCALE, PWM_PRESCALE);
		pwm_write(index, (brightness << 8) | PWM_MOD_MAX, PWM_CNT);
		pwm_write(index, PWM_REG_MSK, PWM_PAT_LOW);
		pwm_write(index, PWM_REG_MSK, PWM_PAT_HIG);
		pwm_write(index, PWM_ENABLE, PWM_PRESCALE);
		printf("sprd backlight power on. pwm_index=%d  brightness=%d\n", index, brightness);
	}

	return;
}
Пример #14
0
void    vex_default_routine(void)

{
    pwm_write(RIGHT_DRIVE_PORT, rc_read_data(1));
    pwm_write(LEFT_DRIVE_PORT, rc_read_data(2));
    /* Drive motors face opposite directions, so reverse left side. */
    pwm_write(3, -rc_read_data(3));
    pwm_write(4, rc_read_data(4));

    /* Handle Channel 5 receiver button */
    if (rc_read_data(5) < BUTTON_REV_THRESH)
	pwm_write(5, MOTOR_FULL_FWD);
    else if (rc_read_data(5) > BUTTON_FWD_THRESH)
	pwm_write(5, MOTOR_STOP);
    else
	pwm_write(5, MOTOR_STOP);
	
    /* Handle Channel 6 receiver button */
    if (rc_read_data(6) < BUTTON_REV_THRESH)
	pwm_write(6, MOTOR_FULL_FWD);
    else if (rc_read_data(6) > BUTTON_FWD_THRESH)
	pwm_write(6, MOTOR_STOP);
    else
	pwm_write(6, MOTOR_STOP);

    /* When Jumper 15 is on CONFIGURATION C is selected */
    if (io_read_digital(15) == DIGITAL_IN_CLOSED)
    {
	/* CONFIGURATION C */
	pwm_write(7, pwm_get(2));
	pwm_write(8, pwm_get(3));
    }
    else
    {
	/* CONFIGURATION A & B */
	pwm_write(7, ~pwm_get(5));
	pwm_write(8, ~pwm_get(6));
    } 
}
Пример #15
0
void indica_sector(float elegir_sector)
{

    switch((int)elegir_sector)
    {
    case 1:
        T1= sqrt_3_2 * V_a - 1* uno_sqrt_2 * V_b;
        T2= sqrt_2 	 * V_b;
        T0= (float)1 - T1 - T2;
        S1_pwm = (float)350 * (T1 + T2 + (float)0.5*T0);
        S3_pwm = (float)350 * (T2 + (float)0.5*T0);
        S5_pwm = (float)350 * ((float)0.5*T0);
        set_pwm_comp();
        break;

    case 2:
        T1= 	sqrt_3_2 * V_a + uno_sqrt_2 * V_b;
        T2= (float)-1 * sqrt_3_2 * V_a + uno_sqrt_2 * V_b;
        T0= (float)1 - T1 - T2;
        S1_pwm=(float)350*(T1 + (float)0.5*T0);
        S3_pwm=(float)350*(T1 + T2 + (float)0.5*T0);
        S5_pwm=(float)350*((float)0.5*T0);
        set_pwm_comp();
        break;

    case 3:
        T1 = sqrt_2 * V_b;
        T2 = (float)-1 * sqrt_3_2 * V_a - uno_sqrt_2 * V_b;
        T0 = (float)1 - T1 - T2;
        S1_pwm = (float)350 * ((float)0.5 * T0);
        S3_pwm = (float)350 *(T1 + T2 + (float)0.5 * T0);
        S5_pwm = (float)350 *(T2 + (float)0.5 * T0);
        set_pwm_comp();
        break;

    case 4:
        T1 = - sqrt_3_2 * V_a + uno_sqrt_2 * V_b;
        T2 = - sqrt_2   * V_b;
        T0 = (float)1 - T1 - T2;
        S1_pwm = (float)350 * ((float)0.5 * T0);
        S3_pwm = (float)350 * (T1 + (float)0.5 * T0);
        S5_pwm = (float)350 * (T1 + T2 + (float)0.5 * T0);
        set_pwm_comp();
        break;

    case 5:
        T1 = - sqrt_3_2 * V_a 	-  uno_sqrt_2 * V_b;
        T2 =   sqrt_3_2 * V_a 	-  uno_sqrt_2 * V_b;
        T0 = (float)1 - T1 - T2;
        S1_pwm = (float)350 * (T2 + (float)0.5 * T0);
        S3_pwm = (float)350 * ((float)0.5 * T0);
        S5_pwm = (float)350 * (T1 + T2 + (float)0.5 * T0);
        set_pwm_comp();
        break;

    case 6:
        T1= -  sqrt_2   * V_b;
        T2=     sqrt_3_2 * V_a + uno_sqrt_2 * V_b;
        T0= (float)1 - T1 - T2;
        S1_pwm=(float)350 * (T1 + T2 + (float)0.5 * T0);
        S3_pwm=(float)350 * ((float)0.5 * T0);
        S5_pwm=(float)350 * (T1 + (float)0.5 * T0);
        set_pwm_comp();
        break;
    }


    pwm_write((int)S1_pwm, (int)S2_pwm, (int)S3_pwm, (int)S4_pwm, (int)S5_pwm, (int)S6_pwm);

}
Пример #16
0
void    autonomous_routine1(void)

{
    unsigned int            sonar_distance = 0;
    static unsigned long    elapsed_time,
			    old_time = 0,
			    start_time;
    
    controller_begin_autonomous_mode();
    elapsed_time = start_time = SYSTEM_TIMER_SECONDS();
    
    /*
     *  An autonomous loop with sensor input.  Loop terminates when
     *  a button sensor on port 5 is pressed, or the 20 second time
     *  period runs out.
     */
    
    forward1(MOTOR_POWER1); 
    while ( (elapsed_time - start_time) < ROUTINE1_DURATION )
    {
	sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
	
	if ( (sonar_distance < ROUTINE1_SONAR_DISTANCE) ||
	     (io_read_digital(BUMPER_LEFT_PORT) == 0) ||
	     (io_read_digital(BUMPER_RIGHT_PORT) == 0) )
	{
	    reverse1(MOTOR_POWER1);
	    delay_msec(ROUTINE1_BACKUP_MS);
	    sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
	    spin1(MOTOR_POWER1, ROUTINE1_SPIN_MS);
	    forward1(MOTOR_POWER1);
	    sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
	}
	
	elapsed_time = SYSTEM_TIMER_MS();
	
	/* Adjust sonar direction every 40 ms */
	if ( elapsed_time % ROUTINE1_SONAR_SERVO_DELAY == 0 )
	    sonar_scan(SONAR_SERVO_PORT);
	
	/* Produce debug output once per second */
	elapsed_time /= MS_PER_SEC;
	if ( elapsed_time > old_time )
	{
	    old_time = elapsed_time;
	    /*
	    if ( rc_new_data_available() )
	    {
		DPRINTF("ET: %ld  RC: %x  A[1,2]: %x %x  "
		    "D[5,6]: %d %d  Shaft I[%d,%d]: %d %d Sonar[%d]: %d\n",
		    elapsed_time - start_time, rc_read_status(),
		    io_read_analog(1),  io_read_analog(2),
		    io_read_digital(5), io_read_digital(6),
		    LEFT_ENCODER_INTERRUPT_PORT, RIGHT_ENCODER_INTERRUPT_PORT,
		    shaft_encoder_read_std(LEFT_ENCODER_INTERRUPT_PORT),
		    shaft_encoder_read_std(RIGHT_ENCODER_INTERRUPT_PORT),
		    SONAR_INTERRUPT_PORT, sonar_distance);
	    }
	    */
	}
    }
    pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
    pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
    controller_submit_data(NO_WAIT);

    controller_end_autonomous_mode();
}
Пример #17
0
static void smart_servo_cw(uint8_t speed)
{
  pwm_write(SMART_SERVO_PW1,0,0,255);
  pwm_write(SMART_SERVO_PW2,speed,0,255);
}
Пример #18
0
void    reverse1(char power)
{
    pwm_write(RIGHT_DRIVE_PORT, -power);
    pwm_write(LEFT_DRIVE_PORT, +power);
    controller_submit_data(WAIT);
}
status_t    shaft_tps_run(shaft_t shafts[], unsigned char count)

{
    static char             active_timers,
			    active_counters;
    static shaft_t          *sp;
    static unsigned long    start_time,
			    elapsed_time;
    
    /*
     *  PID tuning constants (gains)
     *      power += kp * error + ki * error-sum + kd * error-derivitive
     *
     *  Each gain k* is separated into numerator k*n and denominator
     *  k*d to allow fractional gains without resorting to the use
     *  of floating point.  Floating point generally results in a larger,
     *  slower program, and on many processors increases power consumption.
     *
     *  If you increase sample_interval, you should decrease the gain
     *  values proportionally, since they'll be applied less often.
     */
    
    static short            kpn = 1,
			    kpd = 12,
			    kin = 0,
			    kid = 1,
			    kdn = 1,
			    kdd = 1,
			    sample_interval = 50;   /* Milliseconds */

    static unsigned long    actual_ticks,
			    expected_ticks;
    short                   new_power,
			    proportional,
			    derivative,
			    error;
		
    if ( ! VALID_ENCODER_COUNT(count) )
	return OV_BAD_PARAM;
    
    /* Get starting time in ms */
    start_time = TIMER0_ELAPSED_MS;
    
    /* Reset shaft encoder counters */
    for (sp = shafts; sp < shafts + count; ++sp)
    {
	if ( ENCODER_ON_IPORT(sp->interrupt_port) )
	{
	    shaft_encoder_reset(sp->interrupt_port);
	    sp->leave_encoder_on = 1;
	}
	else
	{
	    shaft_encoder_enable(sp->interrupt_port);
	    sp->leave_encoder_on = 0;
	}
	sp->power = 0;
	sp->integral = 0;
    }
    
    /*
     *  Run an ideal PID loop until all time/counter limits are reached.
     */
    
    do
    {
	active_timers = active_counters = count;
	
	/* Get elapsed time in ms */
	elapsed_time = TIMER0_ELAPSED_MS - start_time;
	
	/* Run PID controls at regular intervals */
	if ( elapsed_time % sample_interval == 0 )
	{
	    /*
	     *  Do the more expensive calculation in the initializer
	     *  and the simple one in the comparison to save a few
	     *  cycles in each iteration.
	     */
	    for (sp = shafts + count - 1; sp >= shafts; --sp)
	    {
		/* Check timer and counter */
		if ( elapsed_time >= sp->timer_limit )
		    --active_timers;
		
		actual_ticks = SHAFT_ENCODER_READ_STD(sp->interrupt_port);
		if ( actual_ticks >= sp->tick_limit )
		    --active_counters;
		
		/* Where should we be at this time? */
		expected_ticks = ABS(sp->tps) * elapsed_time / MS_PER_SEC;
		
		error = expected_ticks - actual_ticks;
		
		/* Proportional adjustment = kp * error */
		proportional = kpn * error / kpd;
		
		/*
		 *  Integral (sum of previous errors) accelerates big
		 *  adjustments but can also increase overshoot, so use
		 *  a cheap method (multiply by 2/3) to exponentially
		 *  decay the weight of old errors along the way.
		 */
		sp->integral = (sp->integral * 2 / 3 + (kin * error)) / kid;
		
		/* Derivative */
		derivative = kdn * (error - sp->previous_error) / kdd;
		sp->previous_error = error;
		
		/* Adjust power */
		new_power = ABS(sp->power) + 
		    proportional + sp->integral + derivative;
		sp->power = new_power > 127 ? 127 :
		    (new_power < 0 ? 0 : new_power);
		if ( sp->tps < 0 )
		    sp->power = -sp->power;
	    
		DPRINTF("t: %6lu  m: %u  et: %ld  at: %ld  p: %d  i: %d  d: %d  np: %d %d\n",
			elapsed_time, sp->motor_port,
			expected_ticks, actual_ticks, proportional,
			sp->integral, derivative, new_power, sp->power);
		pwm_write(sp->motor_port, sp->power);
	    }
	    controller_submit_data(WAIT);
	}
    }   while ( active_timers || active_counters );
    
    for (sp = shafts; sp < shafts + count; ++sp)
	if ( !sp->leave_encoder_on )
	    shaft_encoder_disable(sp->interrupt_port);
    return OV_OK;
}
Пример #20
0
void smart_servo_stop(void)
{
  pwm_write(SMART_SERVO_PW1,255,0,255);
  pwm_write(SMART_SERVO_PW2,255,0,255);
}