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); }