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();
}
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();
}
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);
}
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();
}
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);
}
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);
}
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);
}
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);
}
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");
}
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);
}
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);
}
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));
}
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;
}
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));
}
}
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);
}
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();
}
static void smart_servo_cw(uint8_t speed)
{
pwm_write(SMART_SERVO_PW1,0,0,255);
pwm_write(SMART_SERVO_PW2,speed,0,255);
}
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;
}
void smart_servo_stop(void)
{
pwm_write(SMART_SERVO_PW1,255,0,255);
pwm_write(SMART_SERVO_PW2,255,0,255);
}
