Помогите подружить vl53l0x и FlexiTimer2
- Войдите на сайт для отправки комментариев
Сб, 04/04/2020 - 15:40
Пожалуйста подскажите что делаю не так:
#include <FlexiTimer2.h>
#include <VL53L0X.h>
#include <Wire.h>
VL53L0X sensor;
float distance;
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("initialization");
FlexiTimer2::set(1500, radar);
FlexiTimer2::start();
sensor.setTimeout(500);
sensor.init();
}
void loop()
{
// radar();
// delay(1000);
}
void radar() {
Serial.println("radar");
distance = sensor.readRangeSingleMillimeters() / 10;
Serial.print("Distance = ");
Serial.println(distance);
}
Зависает и не хочет мерять растояние при вызове через FlexiTimer2, в com-порт отдается так:
----
initialization
14:26:12.258 -> ra
----
А если строки 15 и 16 закоментировать, а в loop() разкоментировать - то отлично все меряет.
Спасибо.
библиотека FlexTimer запускает вашу процедуру через прерывание - а это значит что внутри запускаемой процедуры, в данном случае процедуры radar - никакие другие прерывания использовать нельзя. А VL53 как раз именно через прерывания и работает.
Вывод - вместе эти процедуры использовать не выйдет.
непонятно другое - нафига вам вообще понадобился ФлексТаймер? - неужели всего лишь ради того, чтоб запускать радар раз в секунду? - это, простите, как микроскопом гвозди забивать. Выкиньте таймер, напишите 2 строчки через миллис - и все отлично заработает
Эх, уже б давно и сделал и пробовал через миллис, но не пашет оно так:) Это четырехногий паук, у него эти все движения серво-лап через FlexTimer идут, а я хочу прицепить дальномер чтоб он постоянно головой крутил, оценивал ситуацию и коректировал движение в процесе, уже по-всякому пробовал тот дальномер сунуть, пока работает только между движением - когда он стоит. Т.е. остановился, головой прокрутил - пошел дальше. А некрасиво это выглядит.
Эх, уже б давно и сделал и пробовал через миллис, но не пашет оно так:)
показывайте как пробовали, не вижу проблем сделать через миллис
там кода 1387 строк (и большинство не моего:) Как бы если есть доброты настолько чтоб это все глянуть то вот текущее состяние кода: ------, сейчас я пока между шагами всовываю дальномер. А текущее желание это всегда раз в 100мс повернуть серво с дальномером и оценить растояние на новом угле.
Когда он делает шаг вперед: step_forward(1) то на секунду loop останавливается и идет работа только с лапами - millis туда не могу вставить, я пробовал сунуть дальномер в ф-ции какие движут лапами, но он там уже не определяет растояние, наверное причина в том что лапы вызываются FlexTimer-ом.
https://www.youtube.com/watch?v=Px1HZeMtJQY - а это старый вариант когда препятствие прямо перед носом, он оставливается и тогда смотрит куда идти, но так он трется по стенах боком. Пробовал поставить по бокам два ультразвуковых, чтоб постоянно смотрел по сторонам и корректировался, но так тоже не очень красиво:)
вы вполне можете выложить в форум свои 1300 строк - сюда и больше выкладывали. Тогда может не только я, но и кто другой посмотрит.
Только, пожалуйста, спрячьте код под спойлер.
А качать архив и разбираться внем - нет, настолько моя доброта не простирается, извините
#include <Servo.h> //to define and control servos #include <FlexiTimer2.h>//to set a timer to manage all servos #include <Wire.h> #include <VL53L0X.h> #include <NewPing.h> // ultrasonic distance sensor library /* Servos --------------------------------------------------------------------*/ //define 12 servos for 4 legs Servo servo[4][3]; //define servos' ports // const int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} }; const int servo_pin[4][3] = { {22, 23, 24}, {25, 26, 27}, {28, 29, 30}, {31, 32, 33} }; /* Size of the robot ---------------------------------------------------------*/ const float length_a = 55; const float length_b = 81.5; const float length_c = 33; const float length_side = 65.5; const float z_absolute = -20; /* Constants for movement ----------------------------------------------------*/ const float z_default = -50, z_up = -20, z_boot = z_absolute; const float x_default = 62, x_offset = 0; const float y_start = 0, y_step = 40; /* variables for movement ----------------------------------------------------*/ volatile float site_now[4][3]; //real-time coordinates of the end of each leg volatile float site_expect[4][3]; //expected coordinates of the end of each leg float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement float move_speed; //movement speed float speed_multiple = 1.2; //movement speed multiple const float spot_turn_speed = 4; const float leg_move_speed = 8; const float body_move_speed = 3; const float stand_seat_speed = 1; volatile int rest_counter; //+1/0.02s, for automatic rest //functions' parameter const float KEEP = 255; //define PI for calculation const float pi = 3.1415926; /* Constants for turn --------------------------------------------------------*/ //temp length const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2)); const float temp_b = 2 * (y_start + y_step) + length_side; const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2)); const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b); //site for turn const float turn_x1 = (temp_a - length_side) / 2; const float turn_y1 = y_start + y_step / 2; const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha); const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side; int cmd = 1; //for ultrasonic sensor #define MIN_DISTANCE_SIDE 10 #define TRIGGER_PIN_LEFT 39 #define ECHO_PIN_LEFT 38 NewPing sonar_left(TRIGGER_PIN_LEFT, ECHO_PIN_LEFT, 50); int arr_sonar_left_distance = 0; #define TRIGGER_PIN_RIGHT 40 #define ECHO_PIN_RIGHT 41 NewPing sonar_right(TRIGGER_PIN_RIGHT, ECHO_PIN_RIGHT, 50); int arr_sonar_right_distance = 0; //for laser sensor #define MAX_DISTANCE 200 int LIMIT = 20; long randNumber; VL53L0X lasersensor; #define LASER_SERVO_PIN 42 Servo laserServo; int operational_mode = 1; // 1 - autorun, 2 - hunting int DISTANCE_MIN = 20; int DISTANCE_SIDE_MIN = 8; int DISTANCE_HUNTING_BACK = 5; int DISTANCE_HUNTING_STOP = 12; int DISTANCE_HUNTING_LOSS = 50; int DISTANCE_HUNTING_MAX = 90; float arr_laser_distance[9] = { -1, -1, -1, -1, -1, -1, -1, -1, -1}; int num_laser_direction = 9; int laser_direction = 1; long laser_last_rotate_time = 0; long laser_last_measure_time = 0; float laser_angle = 0; int laser_ready_for_use = 0; int laser_next_operation = 1; // 1 - rotate; 2 - measure int radar_run = 0; int servo_service_run = 0; int num_radar = 0; /* ---------------------------------------------------------------------------*/ /* - setup function ---------------------------------------------------------------------------*/ void setup() { //start serial for debug Serial.begin(9600); Serial.println("Robot starts initialization"); laserServo.attach(LASER_SERVO_PIN); laserServo.write(laser_angle); //initialize default parameter set_site(0, x_default - x_offset, y_start + y_step, z_boot); set_site(1, x_default - x_offset, y_start + y_step, z_boot); set_site(2, x_default + x_offset, y_start, z_boot); set_site(3, x_default + x_offset, y_start, z_boot); for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { site_now[i][j] = site_expect[i][j]; } } //start servo service FlexiTimer2::set(20, servo_service); // FlexiTimer2::set(1500, radar); FlexiTimer2::start(); Serial.println("Servo service started"); //initialize servos servo_attach(); Serial.println("Servos initialized"); Serial.println("Robot initialization Complete"); radarNull(); Wire.begin(); lasersensor.init(); lasersensor.setTimeout(500); //lasersensor.startContinuous(); // Start continuous back-to-back mode (take readings as // fast as possible). To use continuous timed mode // instead, provide a desired inter-measurement period in // ms (e.g. sensor.startContinuous(100)). randomSeed(analogRead(0)); Serial.println("setupt stand"); stand(); delay(1000); Serial.println("setup hand_wave(3)"); // hand_wave(3); delay(1000); } void servo_attach(void) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { delay(500); servo[i][j].attach(servo_pin[i][j]); delay(1000); } } } void servo_detach(void) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { servo[i][j].detach(); delay(100); } } } /* - loop function ---------------------------------------------------------------------------*/ void loop() { char keyup; // radar(); if (num_radar == 3) { num_radar = 0; } if (num_radar == 0) { radarFull(); num_radar++; } else { } if (cmd == 1) { num_radar++; } else { num_radar = 0; } /* if (My_Receiver.GetResults(&My_Decoder)) { //Restart the receiver so it can be capturing another code //while we are working on decoding this one. My_Receiver.resume(); My_Decoder.decode(); My_Decoder.DumpResults(); } */ /* step_forward(2); turn_left(1); step_forward(2); turn_left(2); step_forward(2); turn_left(3); step_forward(2); turn_left(4); step_forward(2); turn_right(1); step_forward(2); turn_right(2); step_forward(2); turn_right(3); step_forward(2); turn_right(4); */ if (Serial.available()) // проверка поданных команд { keyup = Serial.read(); if (keyup == '9') { cmd = 9; } if (keyup == '1') { cmd = 1; } if (keyup == '2') { cmd = 2; } if (keyup == '3') { cmd = 3; } if (keyup == '4') { cmd = 4; } if (keyup == '5') { operational_mode = 1; cmd = 1; } if (keyup == '6') { operational_mode = 2; cmd = 1; } } switch (cmd) { case 1: step_forward(1); break; case 2: turn_right(1); break; case 3: step_back(1); break; case 4: turn_left(1); break; case 9: stand(); break; } if (operational_mode == 1) { modeAuto(); } if (operational_mode == 2) { modeHunting(); } } void radarNull() { Serial.println("radarNull"); for (int i = 0; i < 9; i++) { arr_laser_distance[i] = -1; } } void radartest() { Serial.println("radartest"); laserServo.write(floor(millis() / 1000)); } void radar() { Serial.println("radar"); int result; //long laser_last_rotate_time = 0; //long laser_last_measure_time = 0; float delta = 22.5; float distance = 0; distance = lasersensor.readRangeSingleMillimeters() / 10; Serial.print("Distance ("); Serial.print(laser_angle); Serial.print(") = "); Serial.println(distance); arr_laser_distance[int(laser_angle / delta)] = distance; if (laser_angle == 90 && DISTANCE_MIN >= arr_laser_distance[4] && arr_laser_distance[4] > 0 && cmd == 1) { Serial.print("Forvard Distance = "); Serial.println(arr_laser_distance[4]); cmd = 9; Serial.println("STOP!!! SET cmd = 9"); radarNull(); } else { if (laser_angle == 0) { laser_direction = 1; } if (laser_angle == 180) { laser_direction = 0; } if (laser_direction == 1) { laser_angle = laser_angle + delta; } if (laser_direction == 0) { laser_angle = laser_angle - delta; } Serial.print("Rotate Angle: "); Serial.println(laser_angle); laserServo.write(laser_angle); laser_last_measure_time = millis(); } laser_ready_for_use = 1; for (int i = 0; i < 9; i++) { if (arr_laser_distance[i] == -1) { laser_ready_for_use = 0; } } } void radarFull() { Serial.println("radarFull"); int result; //long laser_last_rotate_time = 0; //long laser_last_measure_time = 0; float delta = 22.5; float distance = 0; if (laser_angle == 0) { laser_direction = 1; } if (laser_angle == 180) { laser_direction = 0; } for (int i = 0; i < 8; i++) { distance = lasersensor.readRangeSingleMillimeters() / 10; delay(30); Serial.print("Distance ("); Serial.print(laser_angle); Serial.print(") = "); Serial.println(distance); if (distance > 200) { distance = 200; } arr_laser_distance[int(laser_angle / delta)] = distance; if (laser_direction == 1) { laser_angle = laser_angle + delta; } if (laser_direction == 0) { laser_angle = laser_angle - delta; } Serial.print("Rotate Angle: "); Serial.println(laser_angle); laserServo.write(laser_angle); delay(100); } // и запишу последний шаг distance = lasersensor.readRangeSingleMillimeters() / 10; delay(30); Serial.print("Distance ("); Serial.print(laser_angle); Serial.print(") = "); Serial.println(distance); if (distance > 200) { distance = 200; } arr_laser_distance[int(laser_angle / delta)] = distance; if (DISTANCE_MIN >= arr_laser_distance[4] && arr_laser_distance[4] > 0 && cmd == 1) { Serial.print("Forvard Distance = "); Serial.println(arr_laser_distance[4]); cmd = 9; Serial.println("STOP!!! SET cmd = 9"); // radarNull(); } laser_ready_for_use = 1; } void modeAuto() { int need_cmd_step; if (laser_ready_for_use == 1) { float max_distance = arr_laser_distance[0]; int max_direction = 0; for (int st = 0; st < 9; st++) { if (max_distance < arr_laser_distance[st]) { max_distance = arr_laser_distance[st]; max_direction = st; } } Serial.print("Max: "); Serial.println(max_direction); Serial.print("Need run: "); if (max_distance < DISTANCE_MIN) { // просто поворот radarNull(); step_back(2); need_cmd_step = 7; Serial.print("LEFT, "); Serial.println(need_cmd_step); turn_left(need_cmd_step); cmd = 1; } else if (max_direction == 4) { if (cmd != 1) { radarNull(); } Serial.println("FORWARD"); cmd = 1; } else if (max_direction < 4) { radarNull(); need_cmd_step = 4 - max_direction; Serial.print("RIGHT, "); Serial.println(need_cmd_step); turn_right(need_cmd_step); cmd = 1; } else { radarNull(); need_cmd_step = max_direction - 4; Serial.print("LEFT, "); Serial.println(need_cmd_step); turn_left(need_cmd_step); cmd = 1; } } /* if (DISTANCE_MIN >= arr_laser_distance[4] && arr_laser_distance[4] > 0 && cmd == 1) { Serial.print("ZZZ = "); Serial.println(arr_laser_distance[4]); cmd = 9; Serial.println("SET cmd = 9"); Serial.println("Start servo"); for (int st = 0; st < num_laser_direction; st++) { Serial.print("Angle: "); Serial.println(st * 180 / (num_laser_direction - 1)); laserServo.write(st * 180 / (num_laser_direction - 1)); if (st == 0) { delay(1000); // delay(2500); } else { // delay(2500); delay(200); } float distance; int num_distance = 0; float sum_distance = 0; int num_try = 0; while (num_distance < 3) { delay(20); distance = lasersensor.readRangeSingleMillimeters() / 10; Serial.print(st); Serial.print("="); Serial.println(distance); if (distance < 1 && num_try > 10) { distance = 200; } if (distance > 1) { sum_distance += distance; num_distance++; } num_try++; } distance = sum_distance / num_distance; arr_laser_distance[st] = distance; } float min_distance = arr_laser_distance[0]; float max_distance = arr_laser_distance[0]; int min_direction = 0; int max_direction = 0; for (int st = 0; st < num_laser_direction; st++) { Serial.print("Ping ("); Serial.print(st); Serial.print(" / "); Serial.print("): "); Serial.print(arr_laser_distance[st]); Serial.println("cm"); if (min_distance > arr_laser_distance[st]) { min_distance = arr_laser_distance[st]; min_direction = st; } if (max_distance < arr_laser_distance[st]) { max_distance = arr_laser_distance[st]; max_direction = st; } } Serial.print("Min: "); Serial.println(min_direction); Serial.print("Max: "); Serial.println(max_direction); laserServo.write(90); delay(100); Serial.print("Need run: "); if (max_distance < DISTANCE_MIN) { // просто поворот step_back(2); need_cmd_step = 7; Serial.print("LEFT, "); Serial.println(need_cmd_step); turn_left(need_cmd_step); cmd = 1; } else if (max_direction == 4) { Serial.println("FORWARD"); cmd = 1; } else if (max_direction < 4) { need_cmd_step = 4 - max_direction; Serial.print("RIGHT, "); Serial.println(need_cmd_step); turn_right(need_cmd_step); cmd = 1; } else { need_cmd_step = max_direction - 4; Serial.print("LEFT, "); Serial.println(need_cmd_step); turn_left(need_cmd_step); cmd = 1; } } */ } //int DISTANCE_HUNTING_BACK = 5; //int DISTANCE_HUNTING_STOP = 12; //int DISTANCE_HUNTING_LOSS = 50; //int DISTANCE_HUNTING_MAX = 90; void modeHunting() { int need_cmd_step; arr_laser_distance[4] = lasersensor.readRangeSingleMillimeters() / 10; Serial.print("distancefront="); Serial.println(arr_laser_distance[4]); if (arr_laser_distance[4] == 0) { arr_laser_distance[4] = lasersensor.readRangeSingleMillimeters() / 10; if (arr_laser_distance[4] == 0) { arr_laser_distance[4] = lasersensor.readRangeSingleMillimeters() / 10; if (arr_laser_distance[4] == 0) { arr_laser_distance[4] = 200; } } } Serial.println(arr_laser_distance[4]); // need correct if (cmd != 1 && arr_laser_distance[4] > DISTANCE_HUNTING_STOP && arr_laser_distance[4] > 0 && arr_laser_distance[4] < DISTANCE_HUNTING_MAX) { cmd = 1; Serial.println("IF 1"); } if (cmd != 9 && arr_laser_distance[4] <= DISTANCE_HUNTING_STOP && arr_laser_distance[4] > 0 && arr_laser_distance[4] < DISTANCE_HUNTING_MAX) { cmd = 9; Serial.println("IF 2"); } if (cmd != 3 && arr_laser_distance[4] <= DISTANCE_HUNTING_BACK && arr_laser_distance[4] > 0 && arr_laser_distance[4] < DISTANCE_HUNTING_MAX) { cmd = 3; Serial.println("IF 3"); } if (cmd != 2 && cmd != 4 && cmd != 9 && arr_laser_distance[4] > DISTANCE_HUNTING_LOSS && arr_laser_distance[4] > 0) { Serial.println("IF 4"); // проверяем cmd = 9; Serial.println("SET cmd = 9"); Serial.println("Start servo"); for (int st = 0; st < num_laser_direction; st++) { Serial.print("Angle: "); Serial.println(st * 180 / (num_laser_direction - 1)); laserServo.write(st * 180 / (num_laser_direction - 1)); if (st == 0) { delay(1000); } else { delay(200); } float distance; int num_distance = 0; float sum_distance = 0; int num_try = 0; while (num_distance < 4) { delay(20); distance = lasersensor.readRangeSingleMillimeters() / 10; Serial.print(st); Serial.print("="); Serial.println(distance); if (distance < 1 && num_try > 15) { distance = 200; } if (distance > 1) { sum_distance += distance; num_distance++; } num_try++; } distance = sum_distance / num_distance; arr_laser_distance[st] = distance; } float min_distance = arr_laser_distance[0]; float max_distance = arr_laser_distance[0]; int min_direction = 0; int max_direction = 0; for (int st = 0; st < num_laser_direction; st++) { Serial.print("Ping ("); Serial.print(st); Serial.print(" / "); Serial.print("): "); Serial.print(arr_laser_distance[st]); Serial.println("cm"); if (min_distance > arr_laser_distance[st]) { min_distance = arr_laser_distance[st]; min_direction = st; } if (max_distance < arr_laser_distance[st]) { max_distance = arr_laser_distance[st]; max_direction = st; } } Serial.print("Min: "); Serial.println(min_direction); Serial.print("Max: "); Serial.println(max_direction); laserServo.write(90); delay(100); Serial.print("Need run: "); if (min_distance > DISTANCE_HUNTING_MAX) { // ничего не нашел need_cmd_step = 8; Serial.print("LEFT, "); Serial.println(need_cmd_step); cmd = 4; turn_left(need_cmd_step); cmd = 1; } else if (min_direction == 4) { Serial.println("FORWARD"); cmd = 1; } else if (min_direction < 4) { need_cmd_step = 4 - min_direction; Serial.print("RIGHT, "); Serial.println(need_cmd_step); cmd = 2; turn_right(need_cmd_step); cmd = 1; } else { need_cmd_step = min_direction - 4; Serial.print("LEFT, "); Serial.println(need_cmd_step); cmd = 4; turn_left(need_cmd_step); cmd = 1; } } } /* - sit - blocking function ---------------------------------------------------------------------------*/ void sit(void) { move_speed = stand_seat_speed; for (int leg = 0; leg < 4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach(); } /* - stand - blocking function ---------------------------------------------------------------------------*/ void stand(void) { Serial.println("RUN STAND"); move_speed = stand_seat_speed; for (int leg = 0; leg < 4; leg++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach(); } /* - spot turn to left - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/ void turn_left(unsigned int step) { Serial.println("RUN LEFT"); move_speed = spot_turn_speed; while (step-- > 0) { if (site_now[3][1] == y_start) { //leg 3&1 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_default); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start, z_up); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&2 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } } } /* - spot turn to right - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/ void turn_right(unsigned int step) { Serial.println("RUN RIGHT"); move_speed = spot_turn_speed; while (step-- > 0) { if (site_now[2][1] == y_start) { //leg 2&0 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_up); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&3 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_up); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } } } /* - go forward - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/ void step_forward(unsigned int step) { Serial.println("RUN FORWARD"); move_speed = leg_move_speed; while (step-- > 0) { if (site_now[2][1] == y_start) { //leg 2&1 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed = body_move_speed; set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed = leg_move_speed; set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&3 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed = body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed = leg_move_speed; set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } } } /* - go back - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/ void step_back(unsigned int step) { Serial.println("RUN BACK"); move_speed = leg_move_speed; while (step-- > 0) { if (site_now[3][1] == y_start) { //leg 3&0 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed = body_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed = leg_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&2 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed = body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); move_speed = leg_move_speed; set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } } } // add by RegisHsu void body_left(int i) { Serial.println("RUN BODY LEFT"); set_site(0, site_now[0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, KEEP, KEEP); set_site(2, site_now[2][0] - i, KEEP, KEEP); set_site(3, site_now[3][0] - i, KEEP, KEEP); wait_all_reach(); } void body_right(int i) { Serial.println("RUN BODY RIGHT"); set_site(0, site_now[0][0] - i, KEEP, KEEP); set_site(1, site_now[1][0] - i, KEEP, KEEP); set_site(2, site_now[2][0] + i, KEEP, KEEP); set_site(3, site_now[3][0] + i, KEEP, KEEP); wait_all_reach(); } void hand_wave(int i) { Serial.println("HAND WAVE"); float x_tmp; float y_tmp; float z_tmp; move_speed = 1; if (site_now[3][1] == y_start) { body_right(15); x_tmp = site_now[2][0]; y_tmp = site_now[2][1]; z_tmp = site_now[2][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(2, turn_x1, turn_y1, 50); wait_all_reach(); set_site(2, turn_x0, turn_y0, 50); wait_all_reach(); } set_site(2, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_left(15); } else { body_left(15); x_tmp = site_now[0][0]; y_tmp = site_now[0][1]; z_tmp = site_now[0][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(0, turn_x1, turn_y1, 50); wait_all_reach(); set_site(0, turn_x0, turn_y0, 50); wait_all_reach(); } set_site(0, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_right(15); } } void hand_shake(int i) { Serial.println("RUN HAND SHAKE"); float x_tmp; float y_tmp; float z_tmp; move_speed = 1; if (site_now[3][1] == y_start) { body_right(15); x_tmp = site_now[2][0]; y_tmp = site_now[2][1]; z_tmp = site_now[2][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(2, x_default - 30, y_start + 2 * y_step, 55); wait_all_reach(); set_site(2, x_default - 30, y_start + 2 * y_step, 10); wait_all_reach(); } set_site(2, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_left(15); } else { body_left(15); x_tmp = site_now[0][0]; y_tmp = site_now[0][1]; z_tmp = site_now[0][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(0, x_default - 30, y_start + 2 * y_step, 55); wait_all_reach(); set_site(0, x_default - 30, y_start + 2 * y_step, 10); wait_all_reach(); } set_site(0, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_right(15); } } /* - microservos service /timer interrupt function/50Hz - when set site expected,this function move the end point to it in a straight line - temp_speed[4][3] should be set before set expect site,it make sure the end point move in a straight line,and decide move speed. ---------------------------------------------------------------------------*/ void servo_service(void) { sei(); static float alpha, beta, gamma; for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { if (abs(site_now[i][j] - site_expect[i][j]) >= abs(temp_speed[i][j])) site_now[i][j] += temp_speed[i][j]; else site_now[i][j] = site_expect[i][j]; } cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alpha, beta, gamma); } rest_counter++; } /* - set one of end points' expect site - this founction will set temp_speed[4][3] at same time - non - blocking function -------------------------------------------------------------------------- -*/ void set_site(int leg, float x, float y, float z) { float length_x = 0, length_y = 0, length_z = 0; if (x != KEEP) length_x = x - site_now[leg][0]; if (y != KEEP) length_y = y - site_now[leg][1]; if (z != KEEP) length_z = z - site_now[leg][2]; float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); temp_speed[leg][0] = length_x / length * move_speed * speed_multiple; temp_speed[leg][1] = length_y / length * move_speed * speed_multiple; temp_speed[leg][2] = length_z / length * move_speed * speed_multiple; if (x != KEEP) site_expect[leg][0] = x; if (y != KEEP) site_expect[leg][1] = y; if (z != KEEP) site_expect[leg][2] = z; } /* - wait one of end points move to expect site - blocking function ---------------------------------------------------------------------------*/ void wait_reach(int leg) { while (1) if (site_now[leg][0] == site_expect[leg][0]) if (site_now[leg][1] == site_expect[leg][1]) if (site_now[leg][2] == site_expect[leg][2]) break; } /* - wait all of end points move to expect site - blocking function ---------------------------------------------------------------------------*/ void wait_all_reach(void) { for (int i = 0; i < 4; i++) wait_reach(i); } /* - trans site from cartesian to polar - mathematical model 2/2 ---------------------------------------------------------------------------*/ void cartesian_to_polar(volatile float & alpha, volatile float & beta, volatile float & gamma, volatile float x, volatile float y, volatile float z) { //calculate w-z degree float v, w; w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2))); v = w - length_c; alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x); //trans degree pi->180 alpha = alpha / pi * 180; beta = beta / pi * 180; gamma = gamma / pi * 180; } /* - trans site from polar to microservos - mathematical model map to fact - the errors saved in eeprom will be add ---------------------------------------------------------------------------*/ void polar_to_servo(int leg, float alpha, float beta, float gamma) { if (leg == 0) { alpha = 90 - alpha; beta = beta; gamma += 90; } else if (leg == 1) { alpha += 90; beta = 180 - beta; gamma = 90 - gamma; } else if (leg == 2) { alpha += 90; beta = 180 - beta; gamma = 90 - gamma; } else if (leg == 3) { alpha = 90 - alpha; beta = beta; gamma += 90; } servo[leg][0].write(alpha); servo[leg][1].write(beta); servo[leg][2].write(gamma); }