void start_motor(void)
{
sbi(PORTD, 5);
// hack to start mirror even if pwn value is too low to start rotation
pwm_ng_set(&pwm, 5000);
wait_ms(500);
pwm_ng_set(&pwm, get_motor_speed());
}
void actuator_init(void)
{
printf_P(PSTR("fork autopos..."));
pwm_ng_set(FORKROT_PWM, 400);
wait_ms(1000);
pwm_ng_set(FORKROT_PWM, 0);
encoders_spi_set_value(FORKROT_ENCODER, 0);
printf_P(PSTR("ok\r\n"));
ballboard.forkrot.on = 1;
}
/* function called when cmd_event is parsed successfully */
static void cmd_event_parsed(void *parsed_result, void *data)
{
u08 bit=0;
struct cmd_event_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("all"))) {
bit = 0xFF;
if (!strcmp_P(res->arg2, PSTR("on")))
ballboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off")))
ballboard.flags &= bit;
else { /* show */
printf_P(PSTR("encoders is %s\r\n"),
(DO_ENCODERS & ballboard.flags) ? "on":"off");
printf_P(PSTR("cs is %s\r\n"),
(DO_CS & ballboard.flags) ? "on":"off");
printf_P(PSTR("bd is %s\r\n"),
(DO_BD & ballboard.flags) ? "on":"off");
printf_P(PSTR("power is %s\r\n"),
(DO_POWER & ballboard.flags) ? "on":"off");
printf_P(PSTR("errblock is %s\r\n"),
(DO_ERRBLOCKING & ballboard.flags) ? "on":"off");
}
return;
}
if (!strcmp_P(res->arg1, PSTR("encoders")))
bit = DO_ENCODERS;
else if (!strcmp_P(res->arg1, PSTR("cs"))) {
bit = DO_CS;
}
else if (!strcmp_P(res->arg1, PSTR("bd")))
bit = DO_BD;
else if (!strcmp_P(res->arg1, PSTR("power")))
bit = DO_POWER;
else if (!strcmp_P(res->arg1, PSTR("errblock")))
bit = DO_ERRBLOCKING;
if (!strcmp_P(res->arg2, PSTR("on")))
ballboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off"))) {
if (!strcmp_P(res->arg1, PSTR("cs"))) {
pwm_ng_set(ROLLER_PWM, 0);
pwm_ng_set(FORKTRANS_PWM, 0);
pwm_ng_set(FORKROT_PWM, 0);
pwm_ng_set(BEACON_PWM, 0);
}
ballboard.flags &= (~bit);
}
printf_P(PSTR("%s is %s\r\n"), res->arg1,
(bit & ballboard.flags) ? "on":"off");
}
/* called by CS perdiodically (current limit) */
void elevator_manage(void)
{
static int32_t oldpos = 0;
int32_t pos, maxcmd, speed, cmd;
cmd = elevator_cmd;
pos = encoders_microb_get_value(ELEVATOR_ENC);
speed = pos - oldpos;
if (speed > 0 && cmd < 0)
maxcmd = elevator_k1;
else if (speed < 0 && cmd > 0)
maxcmd = elevator_k1;
else {
speed = ABS(speed);
maxcmd = elevator_k1 + elevator_k2 * speed;
}
if (cmd > maxcmd)
cmd = maxcmd;
else if (cmd < -maxcmd)
cmd = -maxcmd;
pwm_ng_set(ELEVATOR_PWM, cmd);
oldpos = pos;
}
/* function called when cmd_pwm is parsed successfully */
static void cmd_pwm_parsed(void * parsed_result, void * data)
{
#ifdef HOST_VERSION
printf("not implemented\n");
#else
void * pwm_ptr = NULL;
struct cmd_pwm_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("1(4A)")))
pwm_ptr = &gen.pwm1_4A;
else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
pwm_ptr = &gen.pwm2_4B;
else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
pwm_ptr = &gen.pwm3_1A;
else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
pwm_ptr = &gen.pwm4_1B;
else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
pwm_ptr = &gen.servo1;
else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
pwm_ptr = &gen.servo2;
else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
pwm_ptr = &gen.servo3;
else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
pwm_ptr = &gen.servo4;
if (pwm_ptr)
pwm_ng_set(pwm_ptr, res->arg2);
printf_P(PSTR("done\r\n"));
#endif
}
void beacon_calibre_pos(void)
{
sensorboard.flags &= ~DO_CS;
/* init beacon pos */
pwm_ng_set(BEACON_PWM, 100);
/* find rising edge of the mirror*/
wait_ms(100);
while (sensor_get(BEACON_POS_SENSOR));
wait_ms(100);
while (!sensor_get(BEACON_POS_SENSOR));
pwm_ng_set(BEACON_PWM, 0);
beacon_reset_pos();
pid_reset(&sensorboard.beacon.pid);
encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
cs_set_consign(&sensorboard.beacon.cs, 0);
sensorboard.flags |= DO_CS;
}
void pwm_set_and_save(void *pwm, int32_t val)
{
/* we need to do the saturation here, before saving the
* value */
if (val > 4095)
val = 4095;
if (val < -4095)
val = -4095;
if (pwm == LEFT_PWM)
mainboard.pwm_l = val;
else if (pwm == RIGHT_PWM)
mainboard.pwm_r = val;
pwm_ng_set(pwm, val);
}
/* function called when cmd_pwm is parsed successfully */
static void cmd_pwm_parsed(void * parsed_result, void * data)
{
void * pwm_ptr = NULL;
struct cmd_pwm_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("1(1A)")))
pwm_ptr = &gen.pwm1_1A;
else if (!strcmp_P(res->arg1, PSTR("2(1B)")))
pwm_ptr = &gen.pwm2_1B;
else if (!strcmp_P(res->arg1, PSTR("3(3A)")))
pwm_ptr = &gen.pwm3_3A;
else if (!strcmp_P(res->arg1, PSTR("4(3B)")))
pwm_ptr = &gen.pwm4_3B;
if (pwm_ptr)
pwm_ng_set(pwm_ptr, res->arg2);
printf_P(PSTR("done %p\r\n"), pwm_ptr);
}
void beacon_reset_pos(void)
{
pwm_ng_set(BEACON_PWM, 0);
encoders_spi_set_value(BEACON_ENCODER, 0);
}
void beacon_stop(void)
{
sensorboard.beacon.on = 0;
pwm_ng_set(BEACON_PWM, 0);
}
/* called every 5 ms */
static void do_cs(void *dummy)
{
static uint16_t cpt = 0;
static int32_t old_a = 0, old_d = 0;
wheel_update_value();
/* robot system, conversion to angle,distance */
if (mainboard.flags & DO_RS) {
int16_t a,d;
rs_update(&mainboard.rs); /* takes about 0.5 ms */
/* process and store current speed */
a = rs_get_angle(&mainboard.rs);
d = rs_get_distance(&mainboard.rs);
mainboard.speed_a = a - old_a;
mainboard.speed_d = d - old_d;
old_a = a;
old_d = d;
}
/* control system */
if (mainboard.flags & DO_CS) {
if (mainboard.angle.on)
cs_manage(&mainboard.angle.cs);
if (mainboard.distance.on)
cs_manage(&mainboard.distance.cs);
if (mainboard.fessor.on) {
cs_manage(&mainboard.fessor.cs);
fessor_manage();
}
if (mainboard.wheel.on)
cs_manage(&mainboard.wheel.cs);
if (mainboard.elevator.on) {
cs_manage(&mainboard.elevator.cs);
elevator_manage();
}
}
if ((cpt & 1) && (mainboard.flags & DO_POS)) {
/* about 1.5ms (worst case without centrifugal force
* compensation) */
position_manage(&mainboard.pos);
}
if (mainboard.flags & DO_BD) {
bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
}
if (mainboard.flags & DO_TIMER) {
uint8_t second;
/* the robot should stop correctly in the strat, but
* in some cases, we must force the stop from an
* interrupt */
second = time_get_s();
if (second >= MATCH_TIME + 2) {
pwm_ng_set(LEFT_PWM, 0);
pwm_ng_set(RIGHT_PWM, 0);
printf_P(PSTR("END OF TIME\r\n"));
while(1);
}
}
/* brakes */
if (mainboard.flags & DO_POWER)
BRAKE_OFF();
else
BRAKE_ON();
cpt++;
}
static void wheel_set(void *dummy, int32_t val)
{
if (val < 0)
val = 0;
pwm_ng_set(WHEEL_PWM, val);
}
/* init fessor position at beginning */
void fessor_autopos(void)
{
pwm_ng_set(FESSOR_PWM, -500);
wait_ms(3000);
pwm_ng_set(FESSOR_PWM, 0);
}
/* init elevator position at beginning */
void elevator_autopos(void)
{
pwm_ng_set(ELEVATOR_PWM, 300);
wait_ms(4000);
pwm_ng_set(ELEVATOR_PWM, 0);
}
void set_motor_speed(uint16_t speed)
{
pwm_ng_set(&pwm, speed);
eeprom_update_word(&eep_motor_speed, speed);
}
