static void
wait_until_start_position(void)
{
unsigned pin_mask = WB_PIN_MASK(HOME_1_PIN) | WB_PIN_MASK(HOME_2_PIN);
struct timespec start_waiting;
fprintf(stderr, "waiting for start\n");
nano_gettime(&start_waiting);
while (pin_mask != 0) {
unsigned max_ms = RETURN_TO_START_MAX_MS;
struct timespec now;
unsigned pin;
nano_gettime(&now);
if (nano_elapsed_ms(&now, &start_waiting) >= max_ms) {
fprintf(stderr, "Failed to return to start!\n");
motor_stop(0);
motor_stop(1);
return;
}
max_ms -= nano_elapsed_ms(&now, &start_waiting);
pin = wb_wait_for_pins_timeout(pin_mask, 0, max_ms);
if (pin) {
pin_mask &= ~WB_PIN_MASK(pin);
fprintf(stderr, "at start %d\n", pin == HOME_1_PIN ? 0 : 1);
if (pin == HOME_1_PIN) motor_stop(0);
else motor_stop(1);
}
}
}
void find_target() {
clear_screen();
print_string("Targg");
motor_spin_left();
left_count = 0;
while (get_range() > MAX_RANGE && left_count < 5 * NINETY_DEGREES) {
clear_screen();
print_num(get_range());
_delay_ms(20);
}
motor_stop();
if (left_count >= 5 * NINETY_DEGREES) {
motor_forward();
left_count = 0;
while (left_count < 90);
motor_stop();
}
else {
motor_spin_right();
_delay_ms(20);
motor_stop();
state = &attack;
}
}
void motor_stop_all()
{
motor_stop(MOTOR_1);
motor_stop(MOTOR_2);
motor_stop(MOTOR_3);
motor_stop(MOTOR_4);
}
void motor_reset() {
/* Begin with resetting axis Z if no reset is already in progress. It is
* imperative to first retract on axis Z separately from the others. Once
* that has been dealt with, axes X and Y may follow. */
if(bit_is_clear(motor_status, MTR_RESET)) {
motor_stop();
motor_status |= _BV(MTR_RESET) | _BV(MTR_IS_Z);
setup_axis(AXIS_Z, MTR_INC);
LOCK_DISABLE();
motor_start();
/* Axis Z has been reset. Now, axes X and Y may follow. */
} else if(bit_is_set(motor_status, MTR_RESET_Z_DONE)) {
/* Remove flag denoting axis Z is resetting. */
motor_status &= ~(_BV(MTR_IS_Z) | _BV(MTR_RESET_Z_DONE));
/* Reset axes X and Y. */
setup_axis(AXIS_Y, MTR_DEC);
setup_axis(AXIS_X, MTR_DEC);
LOCK_DISABLE(); /* Manually enable PWM propagation. */
motor_start();
/* All resetting stages have been completed. Reset #cur_pos and flags. */
} else if(bit_is_set(motor_status, MTR_RESET_X_DONE)
&& bit_is_set(motor_status, MTR_RESET_Y_DONE)) {
cur_pos.x = 0;
cur_pos.y = 0;
cur_pos.z = max_pos.z - 1;
motor_stop();
LOCK_ENABLE();
/* If this resetting cycle was initiated as a response to a limit being
* engaged while under normal motor operation, retry reaching #new_pos
* anew. */
if(bit_is_set(motor_status, MTR_LIMIT)) {
if(motor_update()) {
motor_stop();
MTR_CALL(cur_pos, MTR_EVT_OK);
}
} else {
new_pos = cur_pos;
MTR_CALL(cur_pos, MTR_EVT_OK);
}
motor_status &= ~(_BV(MTR_RESET) | _BV(MTR_LIMIT)
| _BV(MTR_RESET_X_DONE) | _BV(MTR_RESET_Y_DONE));
motor_status |= _BV(MTR_IS_RST_FRESH);
/* Reset is in progress. */
} else {
}
}
static void event(int evt)
{
switch (evt) {
case EVT_FRONT_BLOCKED:
puts("front blocked");
break;
case EVT_BACK_BLOCKED:
puts("back blocked");
break;
case EVT_FRONT_FREE:
puts("front free");
break;
case EVT_BACK_FREE:
puts("back free");
break;
default:
puts("unkown");
}
if ((behavior == 0) || (behavior == 1)) {
/* start condition */
if ((state == 0) && (evt == EVT_FRONT_FREE)) {
puts("evt: start");
speed = CONF_BRAIN_SPEED;
motor_set(&mot, speed);
}
/* going forward, hitting obstacle -> steer full and go backwards */
else if ((state == 0) && (evt == EVT_FRONT_BLOCKED)) {
motor_stop(&mot);
puts("evt: hit forward obstacle");
dir = (behavior == 0) ? -1000 : 1000;
speed = -CONF_BRAIN_SPEED;
brain_steer(dir);
motor_set(&mot, speed);
state = 1;
}
/* going backwards, hitting obstacle */
else if ((state == 1) && (evt == EVT_BACK_BLOCKED)) {
motor_stop(&mot);
puts("evt: hit backwards obstacle");
dir = 0;
speed = CONF_BRAIN_SPEED;
brain_steer(dir);
motor_set(&mot, speed);
state = 0;
}
}
}
void brain_ctrl(int16_t speed, int16_t dir)
{
if ((speed > 0) && front_blocked) {
motor_stop(&mot);
return;
}
if ((speed < 0) && back_blocked) {
motor_stop(&mot);
return;
}
brain_set_speed(speed);
brain_steer(dir);
}
static void joust(void *picked_winner_as_vp, lights_t *l, unsigned pin)
{
unsigned picked_winner = (unsigned) picked_winner_as_vp;
unsigned want_winner = picked_winner == WIN_2_PIN;
unsigned winner_id = random_number_in_range(0, 1);
stop_stop(pick_stop);
fprintf(stderr, "Starting joust\n");
lights_blink_one(l, pin);
track_play_asynchronously(jousting, stop);
motor_forward(0, 1);
motor_forward(1, 1);
ms_sleep(WIN_MS);
stop_stop(stop);
track_play_asynchronously(crash, stop);
motor_forward(0, TROT_DUTY);
motor_forward(1, TROT_DUTY);
ms_sleep(TROT_MS);
stop_stop(stop);
motor_stop(0);
motor_stop(1);
fprintf(stderr, "wanted %d got %d\n", want_winner, winner_id);
if (winner_id == 0) {
wb_set(MOTOR_BANK, WINNER_LIGHT_1, 1);
} else {
wb_set(MOTOR_BANK, WINNER_LIGHT_2, 1);
}
if (winner_id == want_winner) {
track_play(winner);
} else {
track_play(loser);
}
ms_sleep(1000);
wb_set(MOTOR_BANK, WINNER_LIGHT_1, 0);
wb_set(MOTOR_BANK, WINNER_LIGHT_2, 0);
lights_off(l);
track_play_asynchronously(beeping, stop);
go_to_start_position();
stop_stop(stop);
}
static void cmd_dc(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" dc <duty cycle>\n"
" dc arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
const float value = atoff(argv[0]);
lowsyslog("Duty cycle %f\n", value);
motor_set_duty_cycle(value, TTL_MS);
}
void brain_ctrl(int16_t speed, int16_t dir)
{
printf("[brain] CTRL set speed %i and dir %i\n", speed, dir);
if ((speed > 0) && front_blocked) {
motor_stop(&mot);
printf("[brain] CTRL FRONT BLOCKED\n");
return;
}
if ((speed < 0) && back_blocked) {
motor_stop(&mot);
printf("[brain] CTRL BACK BLOCKED\n");
return;
}
brain_set_speed(speed);
brain_steer(dir);
}
static void cmd_rpm(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" rpm <RPM>\n"
" rpm arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
long value = (long)atoff(argv[0]);
value = (value < 0) ? 0 : value;
value = (value > 65535) ? 65535 : value;
lowsyslog("RPM %li\n", value);
motor_set_rpm((unsigned)value, TTL_MS);
}
void attack() {
int left, right;
clear_screen();
print_string("Attack");
motor_forward();
while(1) {
left = read_ir_sensor(LEFT);
right = read_ir_sensor(RIGHT);
if (left >= BLACK && right < BLACK) {
first_led_to_hit = LEFT;
break;
}
else if (right >= BLACK && left < BLACK) {
first_led_to_hit = RIGHT;
break;
}
else if (left >= BLACK && right >= BLACK) {
first_led_to_hit = 47;
break;
}
}
motor_stop();
state = &straighten_out;
}
void main (void)
{
init_clock();
while(1)
{
switch (get_key())
{
case 0:
motor_start();
break;
case 1:
motor_stop();
break;
case 2:
drill_down();
break;
case 3:
drill_up();
break;
case 4:
step();
break;
case 5:
drill_hole();
break;
case 6:
ref_pos();
break;
case 7:
do_auto(pattern);
break;
}
}
}
void task_main(void *pvParameters)
{
while(pb_read(pb2) == 0)
{
// wait for button pressed.
}
vTaskDelay(1000);
for (int speed = 1000; speed < 10000; speed = speed + 50)
{
motor_go_forward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_go_backward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_right();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_left();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
}
// pidMotorMoveFor1Cell(85);
while(1)
{
}
}
//stops and indicates completion. pushing HWB starts things up again.
void maze_completed(void)
{
motor_stop();
led_set(0x0F);
while((PIND & BIT(7)))
{
continue;
}
}
void goto_store()
{
forward();
while((GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_2) == Bit_RESET)&&(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_3) == Bit_RESET)
&&(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_4) == Bit_RESET)&&(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_5) == Bit_RESET));
forward_count(400);
motor_stop();
right_count(850);
while((GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_2) == Bit_RESET)||(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_3) == Bit_RESET)
||(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_4) == Bit_RESET)||(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_5) == Bit_RESET))
{
trace_single_line();
}
forward_count(400);
motor_stop();
store();
}
void handle_message(char header, char size, char *data) {
if ((header & 0xC0) == ADDR_HUVUDENHET) {
char type = header & 0x3F;
switch (type) {
case 0x01: // Gyro har roterat klart
motor_stop();
setGyroDone();
gyroModeOFF();
setDistanceModeOn();
break;
case 0x02: // Står på stopplinje
// hantera på något sätt
break;
case 0x03: // tejpsensor data
send_message(0xE1, size, data);
// sets tape_data to the data collected
tape_data = data[0];
tape_data = tape_data << 8;
tape_data += data[1];
break;
case 0x04: // avståndssensor data
send_message(0xE0, size, data);
for(int i = 0; i < size; i+=2) {
distance_data[i/2] = ((data[i]<<8) & 0xFF00) + (data[i+1] & 0x00FF);
}
/*if (reversing) {
char temp = data[2];
data[2] = data[3];
data[3] = temp;
}*/
if(!findingObject) {
uint8_t temp_data[] = {distance_data[5], distance_data[7]}; // low bytes of distance L and R
send_message_to(ADDR_STYRENHET, 0x02, 0x02, (char*) &temp_data);
} else {
uint8_t distanceRight = getTapeDistanceToSide();
char distance[] = {40-distanceRight, distanceRight};
send_message_to(ADDR_STYRENHET, 0x02, 0x02, (char*) &distance);
}
break;
case 0x05: // kört klart till mitten
middle_done = 1;
break;
case 0x12: // Kör autonomt/sluta köra autonomt
autonomSet(data[0]);
break;
default:
// not sure how to handle this...
//char error[] = "H: Invalid type";
//send_message(0xFF, sizeof(error), &error);
break;
}
}
else {
// send to someone else
send_message(header, size, data);
}
}
void AGV_state_init(void)
{
float angle_offset = AGV_status.Direction;
if(AGV_status.runing_towards == 0 && angle_offset > 180)
{
angle_offset -= 360;
}
angle_offset -= AGV_status.runing_towards;
AGV_V_set(0.01);
if( angle_offset > 0)
{
if(angle_offset > 0.5)
{
motor_run(LEFT_MOTOR,CCW);
motor_run(RIGHT_MOTOR,CCW);
}
else
{
AGV_status.init_Directon_flag = 1;
}
}
else
{
if(angle_offset < -0.5)
{
motor_run(LEFT_MOTOR,CW);
motor_run(RIGHT_MOTOR,CW);
}
else
{
AGV_status.init_Directon_flag = 1;
}
}
if(AGV_status.init_Directon_flag)
{
motor_stop(LEFT_MOTOR);
motor_stop(RIGHT_MOTOR);
CON_ENCODE_LEFT->CNT = 0;
CON_ENCODE_RIGHT->CNT = 0;
}
}
void brain_change_behavior(int bval)
{
behavior = bval;
if (((bval == 0) || (bval == 1)) && (speed == 0)) {
motor_set(&mot, CONF_BRAIN_SPEED);
}
else if (bval == 2) {
motor_stop(&mot);
}
}
static void cmd_startstop(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 5000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" startstop <number of cycles> [duty cycle = 0.1]");
return;
}
motor_stop();
const int num_cycles = (int)atoff(argv[0]);
const float dc = (argc > 1) ? atoff(argv[1]) : 0.1;
int current_cycle = 0;
for (; current_cycle < num_cycles; current_cycle++) {
printf("Cycle %d of %d, dc %f...\n", current_cycle + 1, num_cycles, dc);
// Waiting for the motor to spin down
sleep(3);
if (!motor_is_idle()) {
puts("NOT STOPPED");
break;
}
// Starting with the specified duty cycle
motor_set_duty_cycle(dc, TTL_MS);
// Checking if started and stopping
sleep(3);
if (!motor_is_running()) {
puts("NOT RUNNING");
break;
}
motor_stop();
}
printf("Finished %d cycles of %d\n", current_cycle, num_cycles);
}
void AGV_stop(void)
{
motor_stop(LEFT_MOTOR);
motor_stop(RIGHT_MOTOR);
PID_data_V.err_pre_1 = 0;
PID_data_V.err_pre_2 = 0;
PID_data_run.err_pre_1 = 0;
PID_data_run.err_pre_2 = 0;
if(!AGV_status.avoid_obj_warnning_cnt)
{
AGV_status.runing_status = 0;
AGV_status.rotating_status = 0;
AGV_status.updata_waitting_status = 0;
}else
{
AGV_status.accident_stop_flag = 1;
}
}
void go_to_center() {
clear_screen();
print_string("BackUp");
left_count = 0;
motor_backward();
while (left_count < HALF_WAY_DISTANCE);
state = &find_target;
motor_stop();
}
int reader_test() {
rcc_config();
led_debug_config();
motor_config();
reed_config();
led_blue_off();
led_green_off();
uint8_t i;
while(1) {
// forward
motor_forward();
led_blue_on();
reed_delay_left();
// stop
motor_stop();
led_blue_off();
bigDelay();
bigDelay();
bigDelay();
// backward
motor_back();
led_green_on();
reed_delay_right();
// stop
motor_stop();
led_green_off();
bigDelay();
bigDelay();
bigDelay();
}
}
int
main(int argc, char **argv)
{
if (wb_init() < 0) {
fprintf(stderr, "Failed to initialize wb\n");
exit(1);
}
pthread_mutex_init(&lock, NULL);
stop = stop_new();
pick_stop = stop_new();
stop_stopped(pick_stop);
winner = track_new_fatal("jousters_winner.wav");
loser = track_new_fatal("jousters_loser.wav");
jousting = track_new_fatal("jousters_jousting.wav");
crash = track_new_fatal("jousters_crash.wav");
beeping = track_new_fatal("jousters_beeping.wav");
wb_set_pull_up(HOME_1_PIN, WB_PULL_UP_UP);
wb_set_pull_up(WIN_1_PIN, WB_PULL_UP_UP);
wb_set_pull_up(HOME_2_PIN, WB_PULL_UP_UP);
wb_set_pull_up(WIN_2_PIN, WB_PULL_UP_UP);
motor_forward(0, 0.5);
motor_forward(1, 0.5);
ms_sleep(1000);
motor_stop(0);
motor_stop(1);
ms_sleep(500);
go_to_start_position();
animation_main(stations);
return 0;
}
void motor_set_cmd(uint8_t chan, uint8_t mode, uint8_t speed)
{
uint16_t s = ((float)speed / 255) * 1000;
if (mode == 2) { // fw
motor_set_dir(chan, MOTOR_FW);
motor_set_speed(MOTOR_1, s);
} else if (mode == 1) { // bw
motor_set_dir(chan, MOTOR_BW);
motor_set_speed(MOTOR_1, s);
} else if (mode == 3) { // brake
motor_set_dir(chan, MOTOR_BRAKE);
motor_stop(MOTOR_1);
}
}
void brain_change_behavior(int bval)
{
printf("[brain] new behavior: %i\n", bval);
behavior = bval;
if ((bval == 0) || (bval == 1)) {
state = 0;
motor_set(&mot, CONF_BRAIN_SPEED);
event(EVT_FRONT_FREE);
}
else if (bval == 2) {
motor_stop(&mot);
printf("[brain] STOPPING MOTOR\n");
}
}
void
task_motor_stop(void *p_arg)
{
OS_ERR err;
OS_MSG_SIZE size;
CPU_TS ts;
CMD_STRU *msg;
while (1)
{
msg = (CMD_STRU*)OSQPend(&StopQ, 0, OS_OPT_PEND_BLOCKING, &size, &ts, &err);
motor_stop();
}
}
void loop() {
if (Serial.available()) {
String readString = "";
while (Serial.available()) {
delay(3);
if (Serial.available() > 0) {
char c = Serial.read();
readString += c;
}
}
if (readString.charAt(0) == 'M') {
switch (readString.charAt(1)) {
case 'f' : case 'F':
motor_forward();
break;
case 'b': case 'B':
motor_backward();
break;
case 'l': case 'L':
motor_turnLeft();
break;
case 'r': case 'R':
motor_turnRight();
break;
case 's': case 'S':
motor_stop();
break;
case 'm': case 'A':
isUltra = !isUltra;
break;
case 'p': case 'P':
char c = readString.charAt(2);
ChangeSpeed((c - '0') * factor + minSpeed);
break;
}
}
} else {
if (isUltra) {
//ultraCarProcess();
}
else {
//motor_stop();
}
}
}
void do_auto(const int *pattern)
{
int *iterator = pattern;
if (!ref_pos())
return;
motor_start();
for (; *iterator != 0xFF; iterator++)
{
n_step(*iterator);
drill_hole();
}
motor_stop();
}
void motor_on(uint8_t level, clock_time_t length)
{
if (level > 16) level = 16;
level *= 2;
if (level == 0)
{
motor_stop(NULL);
}
else
{
MOTORCONTROL = OUTMOD_7;
MOTORLEVEL = level;
if (length > 0)
ctimer_set(&motor_timer, length, motor_stop, NULL);
}
}
void straighten_out() {
clear_screen();
print_string("Fix");
if (first_led_to_hit != 47) {
if (first_led_to_hit == LEFT)
motor_spin_left();
else if (first_led_to_hit == RIGHT)
motor_spin_right();
left_count = 0;
while (left_count < 5);
motor_stop();
}
state = &go_to_center;
}
