1. Motor function description
The R111 prototype is a serial bionic hexapod robot with 12 degrees of freedom. The example in this article implements the walking function of a 12-DOF hexapod robot, including forward, backward, left turn, and right turn.
2. Structure Description
The R111 prototype is composed of six 2-DOF series structures, and the middle is fixed by the double bending of the steering gear and the combination of studs, so as to achieve the structure of a bionic robot.
The 2-DOF serial structure is driven by the steering gear 1 and the steering gear 2, wherein the steering gear 1 realizes the swing of the legs back and forth, and the steering gear 2 realizes the lifting and stretching of the legs. Among them, a parallel four-link ABCD is used as a transmission structure to increase the stroke of the leg and enhance the stability of the leg movement.
3. Principles of Gait
There are two kinds of gaits for the 12-DOF hexapod robot: one is to use triangular gait to move, and the other is to use undulating gait to move.
① Triangular gait
The triangular gait of a 12-degree-of-freedom hexapod bionic robot is to divide the hexapods of the robot into two sets of legs (the front foot on one side of the body, the rear foot and the middle foot on the other side) to swing and support respectively, that is, three legs in a triangle Both are in the swing phase or both are in the support phase. As shown in the picture:
② Fluctuating gait
The undulating gait of the 12-degree-of-freedom hexapod bionic robot is that each leg of the robot moves in sequence, that is, the left (right) side leg moves first, then the right (left) side leg steps, and then the left (right) side next leg Movement, such a cycle completes the movement of undulating gait. As shown in the picture:
Tip: You can refer to the gait of the centipede.
This article will talk about the realization of triangular gait. You can study the wave gait by yourself.
4. Implementation based on STM32 main control board
4.1 Electronic Hardware
In this example, we use the following hardware, please refer to:
| Main Control Board |
STM32 main control board |
| Expansion board |
STM32 expansion board |
| Standard steering gear | |
| Battery | 7.4V lithium battery |
Connect the circuit as shown in the figure below: We first number the steering gear of the mechanism (see the figure below)
Next, connect with the expansion board (see the picture below)
See the table below for the corresponding pin numbers, which will be used in programming.
| Material | Pin number | Material |
Pin Number |
| Servo 0 | PE9 | Servo 6 | PD15 |
| Servo 1 | PE11 |
Servo 7 |
PD14 |
| Servo 2 | PE13 |
Servo 8 |
PD13 |
| Servo 3 | PE14 |
Servo 9 |
PB15 |
| Servo 4 | PC6 |
Servo 10 |
PB14 |
| Servo 5 | PC7 |
Servo 11 |
PB9 |
4.2 Sample program
Programming environment:keil5
Use the Controller to get the attitude parameters and copy them to the program below.
Reference routine for triangular gait progress (USER\test.uvprojx), code of main.c:
/*--------------------------------------------- ---------------------------------------
Copyright Note: Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license. See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by Robotway 2023-02-16 https://www.robotway.com/
------------------------------*/
#include "sys.h"
#include "led.h"
#include "stdio.h"
#include "delay.h"
#include "usart.h"
#include "stdio.h"
#include "core_cm4.h"
#include "string.h"
#include "stdlib.h"
#include "pwm.h"
#include "timer.h"
#include "math.h"
/*
7 1
| |
6----| |----0
| |
9 3
| |
8----| |----2
| |
11 5
| |
10----| |----4
| |
*/
float Ang[12] = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
int Ang_Init[12] = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
float data_f[12] = {0};
void split(char *src,const char *separator,char **dest,int *num);//string splitting function
float sort(float data[12]);//Sorting function (small -> large)
float find_min(float *de);//Find the minimum value function
void Set_Ang(float, float, float, float, float, float, float, float, float, float, float, float);//Set the steering gear rotation angle function
void Steering_Gear_Init(void);//Robot arm initialization position function
void Steering_Gear_Angle(u8 num, float ang);//single steering gear control function
void Steering_Gear_Ang_Pwm1(float data[12]);//Robot arm movement function
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//Set system interrupt priority group 2
delay_init(168);//Initialization delay, 168 is the CPU operating frequency
usart_init(115200); //serial port initialization
LED_Init();//Initialize the LED light
TIM1_PWM_Init(20000-1, 168-1);
TIM4_PWM_Init(20000-1,84-1);
TIM3_PWM_Init(20000-1,84-1);
TIM11_PWM_Init(20000-1, 168-1);
TIM12_PWM_Init(20000-1,84-1);
Steering_Gear_Init();//hexapod position initialization
while(1)
{
Set_Ang(80,85,110,90,110,80,80,90,115,90,70,95);
Steering_Gear_Ang_Pwm1(data_f);
delay_ms(1000);
while (1) {
Set_Ang(90,100,105,75,110,95,90,105,105,75,80,110);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(70,100,70,75,130,95,120,105,120,75,120,110);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(70,70,70,105,130,65,120,75,120,105,120,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(90,70,105,105,110,65,90,75,105,105,80,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(120,70,120,105,60,65,80,75,70,105,70,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(120,100,120,75,60,95,80,105,70,75,70,110);
Steering_Gear_Ang_Pwm1(data_f);
}
}
}
void split(char *src, const char *separator, char **dest, int *num)
{
char *pNext;
int count = 0;
if (src == NULL || strlen(src) == 0)//The string is empty or the end flag is encountered
return;
if (separator == NULL || strlen(separator) == 0)//separator is empty
return;
pNext = strtok(src,separator);//string split
while(pNext != NULL) {
*dest + + = pNext;
+ + count;
pNext = strtok(NULL,separator);
}
*num = count;
}
//Rearrange all members in the array in ascending order, and return the minimum value
float sort(float data1[12])
{
float array[12] = {0};
int i = 0;
int j = 0;
int temp;
for(int i=0; i < 12; i ++ ){
array[i] = data1[i];
}
for ( i = 0; i < 12; i ++ ){
for ( j = 0; j < 11-i; j ++ ){
if(array[j]>array[j + 1]){
temp=array[j];
array[j]=array[j + 1];
array[j+1]=temp;
}
}
}
return find_min(array);
}
// Find the minimum value in the array
float find_min(float *de)
{
if((int)*de != 0)
return *de;
find_min(++de);
}
//Assign value to the array
void Set_Ang(float ang1, float ang2, float ang3, float ang4, float ang5, float ang6, float ang7, float ang8, float ang9, float ang10, float ang11, float ang12)
{
data_f[0] = ang1;
data_f[1] = ang2;
data_f[2] = ang3;
data_f[3] = ang4;
data_f[4] = ang5;
data_f[5] = ang6;
data_f[6] = ang7;
data_f[7] = ang8;
data_f[8] = ang9;
data_f[9] = ang10;
data_f[10] = ang11;
data_f[11] = ang12;
}
//Convert the degrees that need to be moved by the servo between two positions into multiple steps, and then complete it step by step
void Steering_Gear_Ang_Pwm1(float data[12])
{
float den = 0;
float dif[12] = {0};
floatang[12] = {0};
// Calculate the difference between the two positions
for(int i=0; i < 12; i ++ ){
dif[i] = fabs(data[i] - Ang[i]);
}
// get minimum difference
den = sort(dif);
//Calculate the minimum distance of each movement of 12 servos
if((int)den == 0){
for(int i=0; i < 12; i ++ )
ang[i] = 0;
}else{
for(int i=0; i < 12; i ++ ){
ang[i] = (data[i] - Ang[i])/den;
}
for(int i=0; i<den; i ++ ){
for(int j=0; j < 12; j ++ )
Ang[j] + = ang[j];
//Restrict the position of multiple steering gears to prevent the steering gears from stalling
for(int g=0; g < 12; g++ ){
if(Ang[g] < 0)
Ang[g] = 0;
else if(Ang[g] > 180)
Ang[g] = 180;
}
//rudder control
for(int k=0; k<12; k ++ ){
Steering_Gear_Angle(k, Ang[k]);
}
}
}
}
void Steering_Gear_Angle(u8 num, float ang)
{
u32 Ang = 0;
Ang = 500 + ang*2000/180;
switch(num){
case 0:
TIM_SetCompare1(TIM1, Ang);//Modify the comparison value, modify the duty cycle
break;
case 1:
TIM_SetCompare2(TIM1, Ang);//Modify the comparison value, modify the duty cycle
break;
case 2:
TIM_SetCompare3(TIM1, Ang);//Modify the comparison value, modify the duty cycle
break;
case 3:
TIM_SetCompare4(TIM1, Ang);//Modify the comparison value, modify the duty cycle
break;
case 4:
TIM_SetCompare1(TIM3, Ang);//Modify the comparison value, modify the duty cycle
break;
case 5:
TIM_SetCompare2(TIM3, Ang);//Modify the comparison value, modify the duty cycle
break;
case 6:
TIM_SetCompare4(TIM4, Ang);//Modify the comparison value, modify the duty cycle
break;
case 7:
TIM_SetCompare3(TIM4, Ang);//Modify the comparison value, modify the duty cycle
break;
case 8:
TIM_SetCompare2(TIM4, Ang);//Modify the comparison value and modify the duty cycle
break;
case 9:
TIM_SetCompare2(TIM12, Ang);//Modify the comparison value, modify the duty cycle
break;
case 10:
TIM_SetCompare1(TIM12, Ang);//Modify the comparison value, modify the duty cycle
break;
case 11:
TIM_SetCompare1(TIM11, Ang);//Modify the comparison value, modify the duty cycle
break;
default:
break;
}
delay_ms(1);
}
//hexapod starting position
void Steering_Gear_Init(void)
{
for(int i=0; i < 12; i ++ ){
Steering_Gear_Angle(i, Ang_Init[i]);
}
delay_ms(1000);
}
5. Realization based on Arduino main control board
5.1 Electronic hardware
In this example, we use the following hardware, please refer to:
| Main Control Board |
Basra main control board (compatible with Arduino Uno)? |
| Expansion board |
SH-SR servo expansion board |
| Standard steering gear | |
| Battery | 7.4V lithium battery |
Circuit Connection Instructions:
Because there are 12 servos installed on the 12-DOF hexapod robot structure, the SH-SR servo expansion board will be used.
Servo wiring sequence: 1, 3; 4, 5; 6, 8; 11, 13; 15, 21; 24, 26.
5.2 Sample program
Use the Controller to obtain the attitude parameters, and copy them to the following reference routine (robot_walk.ino) for the triangular gait of the 12-DOF hexapod robot, and download it to the main control board:
/*--------------------------------------------- ---------------------------------------
Copyright Note: Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license. See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by Robotway 2023-02-16 https://www.robotway.com/
------------------------------*/
#include <Arduino.h>
#include <avr/pgmspace.h>
#include "Config.h"
#include <Tlc5940.h>
#include <tlc_servos.h>
int count_input = 0;
boolean _b = false;
/************** + + + + + + + + + + + action group array, please paste the converted action group data into the corresponding action group + + + + + + + *****************************/
const PROGMEM int actionInit[] = {
1090, 1790, 1220, 980, 650, 1800, 1020, 1330, 520, 1155, 1260, 2020,
1090, 1790, 1220, 980, 650, 1800, 1020, 1330, 520, 1155, 1260, 2020,
};
const PROGMEM int actionMove[] = {
1660, 1230, 1220, 980, 1280, 1190, 1020, 1330, 1090, 1155, 1260, 1540,
1660, 1790, 1320, 1080, 1280, 1800, 920, 1230, 1090, 1255, 1360, 2020,
1660, 1230, 1320, 1080, 1280, 1190, 920, 1230, 1090, 1255, 1360, 1540,
1090, 1300, 1120, 880, 650, 900, 1120, 1430, 520, 1055, 1160, 1300,
1660, 1300, 1120, 880, 1280, 900, 1120, 1430, 1090, 1055, 1160, 1300,
};
const PROGMEM int actionLeft[] = {
};
const PROGMEM int actionRight[] = {
};
const PROGMEM int actionBack[] = {
};
const PROGMEM int actionDance[] = {
};
/************************** + + + + + ---------Separation line------- - + + + + + + *********************************************** *************/
//Action group array length acquisition function, when adding an action group, it needs to be added in the following format: actPwmNum[Added action group number] = sizeof(Added action group name) / sizeof(Added action group name[0]);
void act_length()
{
actPwmNum[0] = (sizeof(actionMove) / sizeof(actionMove[0]))/servo_num;
actPwmNum[1] = (sizeof(actionLeft) / sizeof(actionLeft[0]))/servo_num;
actPwmNum[2] = (sizeof(actionRight) / sizeof(actionRight[0]))/servo_num;
actPwmNum[3] = (sizeof(actionBack) / sizeof(actionBack[0]))/servo_num;
actPwmNum[4] = (sizeof(actionDance) / sizeof(actionDance[0]))/servo_num;
actPwmNum[5] = (sizeof(actionInit) / sizeof(actionInit[0]))/servo_num;
}
//map mapping function
long map_servo(long x, long in_min, long in_max, long out_min, long out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
//PWM is converted to the function of the servo angle, it needs to be modified when adding action groups
void vlaue2angle(int p, int act)
{
switch(act)
{
case 0: value_cur[p] = map_servo(pgm_read_word_near(actionMove + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 1: value_cur[p] = map_servo(pgm_read_word_near(actionLeft + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 2: value_cur[p] = map_servo(pgm_read_word_near(actionRight + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 3: value_cur[p] = map_servo(pgm_read_word_near(actionBack + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 4: value_cur[p] = map_servo(pgm_read_word_near(actionDance + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 5: value_cur[p] = map_servo(pgm_read_word_near(actionInit + p + servo_num * count_input), 500, 2500, 0, 180); break;
default: break;
}
}
//Steering gear initialization function, the first line of the action group is the steering gear initialization value
void servo_init(int act, int num)
{
if(!_b)
{
for(int i=0;i<servo_num;i++)
{
vlaue2angle(i, act);
tlc_setServo(servo_pin[i], value_cur[i]);
value_pre[i] = value_cur[i];
}
Tlc. update();
//delay(1000);
}
num == 1 ? _b = true : _b = false;
}
//Steering gear movement function, parameters: act: Action group macro definition name; num: Action group execution times, num > 0;
void servo_move(int act, int num)
{
float value_delta[servo_num] = {};
float in_value[servo_num] = {};
servo_init(act, num);
for(int i=0;i< num * actPwmNum[act];i ++ )
{
count_input++;
if(count_input == actPwmNum[act])
{
count_input = 0;
continue;
}
for(int i=0;i<servo_num;i++)
{
vlaue2angle(i, act);
in_value[i] = value_pre[i];
value_delta[i] = (value_cur[i] - value_pre[i]) / frequency;
}
for(int i=0;i<frequency;i++)
{
for(int k=0;k<servo_num;k++)
{
in_value[k] + = value_delta[k];
value_pre[k] = in_value[k];
}
for(int j=0;j<servo_num;j++)
{
tlc_setServo(servo_pin[j], in_value[j]);
delayMicroseconds(servo_speed);
}
Tlc. update();
}
delayMicroseconds(action_delay);
}
}
/**************************************************** *******-------Dividing line--------************************** ***************************/
//Initialization function
void setup() {
Serial.begin(9600); //Open serial communication, baud rate 9600
Tlc.init(0);
tlc_initServos();
act_length();
delay(action_delay);
}
//main function, walk
void loop() {
servo_move(action_move, 6);
delay(100);
}
Note:
Although the above provides a reference routine for you, it does not mean that the robot can run normally after the program is successfully burned directly, and you need to adjust some parameters or structure. Debug hints:
(1) Adjust the servo angle of each joint to be consistent with the program during installation;
(2) The initial angle in the program is adjusted to the angle during installation, and then the angles of other actions are adjusted according to the angle difference of previous movements;
(3) Pay attention to the port number corresponding to the servo on each leg, and the movement needs to correspond to the triangle gait;
(4) The movement speed of the robot can be controlled by adjusting the delay parameter.
6. Data download
Data content:
①Gait planning-STM32 routine source code
②Gait planning-Arduino routine source code
For details, see 12 degrees of freedom hexapod-gait planning