├── cad(step) ├── puppy-bot8.STEP ├── new-puppy-bot12.STEP └── README.md ├── puppy_bot ├── .gitignore ├── include │ ├── myimu.h │ ├── command.h │ ├── myservo.h │ ├── PID.h │ ├── motion.h │ ├── kine.h │ └── README ├── src │ ├── main.cpp │ ├── myservo.cpp │ ├── PID.cpp │ ├── myimu.cpp │ ├── kine.cpp │ ├── command.cpp │ └── motion.cpp ├── test │ └── README ├── platformio.ini ├── lib │ └── README └── .travis.yml ├── README.md └── sim5link └── sim5link.py /cad(step)/puppy-bot8.STEP: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/servo-dogbot/HEAD/cad(step)/puppy-bot8.STEP -------------------------------------------------------------------------------- /cad(step)/new-puppy-bot12.STEP: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wuyangning/servo-dogbot/HEAD/cad(step)/new-puppy-bot12.STEP -------------------------------------------------------------------------------- /puppy_bot/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /puppy_bot/include/myimu.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "PID.h" 3 | 4 | 5 | 6 | void init_imu(); 7 | void get_rpy(); 8 | void get_data(); 9 | 10 | extern PID ROLL,PITCH; 11 | 12 | -------------------------------------------------------------------------------- /cad(step)/README.md: -------------------------------------------------------------------------------- 1 | - pupppy-bot12是最早的12舵机版本，预留的空间比较少，放不下太多东西。 2 | 3 | - puppy-bot8是后面修改的8舵机版本，有更大的空间，细节也更完善。可以直接内置两节18650(建议使用动力18650)。 4 | 5 | - new-puppy-bot12新画的12舵机版本 6 | 7 | - 舵机尺寸是标准的9g。建议使用数字舵机，速度不低于0.1s/60°。 -------------------------------------------------------------------------------- /puppy_bot/include/command.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "kine.h" 3 | 4 | #define cm_start 's' //起始符 5 | #define cm_end '\n' //终止符 6 | #define cm_sep '\n' //分割符 7 | 8 | 9 | extern KINE leg[4]; 10 | extern const uint8 servo_num[8]; 11 | 12 | void init_command(); //清空指令 13 | 14 | void cli_loop(); 15 | 16 | void process_command(); 17 | 18 | double key_value(char key);//提取字符后的浮点数 19 | 20 | bool command_seen(char key);//判断是否存在字符 -------------------------------------------------------------------------------- /puppy_bot/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | 7 | 8 | 9 | void setup() 10 | { 11 | Serial.begin(115200); 12 | setup_motion(); 13 | 14 | 15 | trajy_para param={0.5,1,40,0,20,20,0}; 16 | trajy_planning(param); 17 | 18 | delay(5000); 19 | gait_transit(0,20); 20 | 21 | 22 | } 23 | 24 | void loop() 25 | { 26 | 27 | gait_loop(); 28 | 29 | //cli_loop(); 30 | 31 | 32 | } 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /puppy_bot/include/myservo.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #ifndef myservo_h 3 | #define myservo_h 4 | 5 | #define pulse_per_rad 9200 //每弧度对应的脉宽 6 | #define SERVO_ENABLE_PIN PB12 //用一个场效应管开关使能舵机 7 | 8 | //实际输出脉宽=k*pulse_per_rad*关节角+b 9 | const int8_t k[8]={-1,-1,1,1,-1,-1,1,1}; 10 | const int b[8]={30252,14572,15268,30393,12695,30730,29939,14620}; 11 | //引脚对应的舵机编号 12 | const uint8 servo_num[8]={PA0,PA1,PA2,PA3,PA6,PA7,PB0,PB1}; 13 | 14 | void init_servo(); 15 | void set_joint_rad(uint8,float); 16 | 17 | #endif -------------------------------------------------------------------------------- /puppy_bot/src/myservo.cpp: -------------------------------------------------------------------------------- 1 | #include "myservo.h" 2 | 3 | void init_servo() 4 | { 5 | Timer2.setPeriod(4000); 6 | Timer3.setPeriod(4000); //PWM:250Hz 7 | 8 | for(uint8 i=0;i<8;i++) 9 | { 10 | pinMode(servo_num[i],PWM); 11 | //pwmWrite(servo_num[i],21000); 12 | } 13 | pinMode(PB12,OUTPUT); 14 | digitalWrite(PB12,HIGH); 15 | } 16 | 17 | void set_joint_rad(uint8 num,float radian) 18 | { 19 | int pul=int(k[num]*pulse_per_rad*radian+b[num]); 20 | pwmWrite(servo_num[num],pul); 21 | 22 | } -------------------------------------------------------------------------------- /puppy_bot/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /puppy_bot/platformio.ini: -------------------------------------------------------------------------------- 1 | ;PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:genericSTM32F103C8] 12 | platform = ststm32 13 | board = genericSTM32F103C8 14 | framework = arduino 15 | 16 | monitor_speed = 115200 17 | upload_protocol = dfu -------------------------------------------------------------------------------- /puppy_bot/include/PID.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #ifndef PID_h 3 | #define PID_h 4 | 5 | class PID 6 | { 7 | public: 8 | int sampletime;//采样周期 9 | float target; //设定值 10 | float current;//输入 11 | float output;//输出 12 | 13 | void set_pid(float,float,float,int,float);//设定pid值 14 | void print_pid();//串口输出pid 15 | void compute();//计算pid 16 | 17 | private: 18 | float kp; 19 | float ki; 20 | float kd; 21 | float dead_zone; 22 | 23 | 24 | 25 | float k[3];//系数 26 | float err[3];//三次的误差 27 | 28 | }; 29 | 30 | #endif 31 | 32 | 33 | -------------------------------------------------------------------------------- /puppy_bot/src/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "PID.h" 2 | 3 | void PID::set_pid(float p,float i,float d,int st,float dz) 4 | { 5 | kp=p; 6 | ki=i; 7 | kd=d; 8 | sampletime=st; 9 | dead_zone=dz; 10 | k[0]=kp+ki*sampletime+kd/sampletime; 11 | k[1]=kp+2*kd/sampletime; 12 | k[2]=kd/sampletime; 13 | 14 | } 15 | 16 | 17 | void PID::print_pid() 18 | { 19 | Serial.print("kp:"); 20 | Serial.print(kp); 21 | Serial.print("ki:"); 22 | Serial.print(kp); 23 | Serial.print("kd:"); 24 | Serial.println(kd); 25 | } 26 | 27 | void PID::compute() 28 | { 29 | err[2]=err[1]; 30 | err[1]=err[0]; 31 | err[0]=target-current; 32 | if(abs(err[0]) THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /puppy_bot/src/myimu.cpp: -------------------------------------------------------------------------------- 1 | #include "myimu.h" 2 | 3 | static uint8 Re_buf[12]; 4 | static uint8 counter=0; 5 | 6 | 7 | //串口陀螺仪GY953 8 | void init_imu() 9 | { 10 | Serial3.begin(115200); 11 | 12 | //输出欧拉角 13 | Serial3.write(0XA5); 14 | Serial3.write(0X45); 15 | Serial3.write(0XEA); 16 | } 17 | 18 | void get_rpy() //获取rpy角 19 | { 20 | get_data(); 21 | //yall=float(Re_buf[8]<<8|Re_buf[9])/91; 22 | //if(yall>180) 23 | //yall-=720; 24 | ROLL.current=float(Re_buf[6]<<8|Re_buf[7])/98; 25 | if(ROLL.current>578.7) 26 | ROLL.current-=668.7; 27 | 28 | PITCH.current=float(Re_buf[4]<<8|Re_buf[5])/98; 29 | if(PITCH.current>578.7) 30 | PITCH.current-=668.7; 31 | PITCH.current=-PITCH.current; 32 | 33 | 34 | } 35 | 36 | //获取陀螺仪数据 37 | void get_data() //获取一个完整的帧 38 | { 39 | while (Serial3.available()) 40 | { 41 | Re_buf[counter]=(unsigned char)Serial3.read(); 42 | if(counter==0&&Re_buf[0]!=0x5A) continue; //如果帧头不对，则重新获取 43 | counter++; 44 | if(counter==11) //接收到下一帧的帧头为止 45 | { 46 | counter=0; 47 | if(Re_buf[0]==0x5A&&Re_buf[11]==0x5A )//如果接收到完整一帧，跳出，否则继续 48 | break; 49 | else 50 | continue; 51 | } 52 | } 53 | } -------------------------------------------------------------------------------- /puppy_bot/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # puppy-bot 2 | ![](https://wx2.sinaimg.cn/mw690/e8d4eb99ly1gduh8t5tcdj20u01404jr.jpg) 3 | 4 | 视频演示：https://www.bilibili.com/video/BV1if4y1U7h3 5 | # 文件目录 6 | - cad(step) 7 | - step模型文件 8 | - puppy_bot 9 | - 源码，使用platformio 10 | - sim5link 11 | - 简易的5杆机构运动学仿真 12 | 13 | # 关于 14 | - 为了把电池塞进去，由原来的12个舵机改成了8个。舵机也直接由单片机驱动。 15 | - 通过修改一些参数就可以切换各种步态，但不是所有步态都能用。这取决于舵机的性能。测试中trot步态是效果最好的。 16 | - 目前只有一个陀螺仪，可以适应不同坡度的平面地形。其他的复杂的地面，落差小一点还可以。 17 | - 五杆机构惯量小，布局起来也很方便。但是非线性比较严重，在一些位置受力和精度会很差。 18 | - 用的是纯位置控制(非常原始的控制方法)。目前只完成步态和一些动作的验证。完整的控制流程和一些保护机制还需要慢慢添加。仅作参考😂。大概流程如下。 19 | 20 | ![](https://wx2.sinaimg.cn/mw690/e8d4eb99ly1gduhn6wd7rj213r0hpt9w.jpg) 21 | 22 | 23 | 24 | # 使用 25 | - 目前是直接用pwm输出给舵机，用的陀螺仪是串口陀螺仪GY952。如果是其他舵机驱动方式或陀螺仪。需要在myimu中先实现欧拉角的输出，在myservo中实现关节角的输出。 26 | - 舵机需要标定，这一步比较麻烦(之后弄个上位机)。舵机脉宽和关节角的之间是一个线性关系，需要在myservo.h中确定。每条腿的连杆和关节角的定义如下(关节角是连杆以绿线为起点，逆时针旋转的角度)： 27 | ![](https://wx4.sinaimg.cn/mw690/e8d4eb99ly1gduj9z8157j20yg0u0qhj.jpg) 28 | - 参数 29 | ```C 30 | //LF,RB,RF,LB的步态相位 31 | float phase[4]={0,0,0.5,0.5} 32 | //腿悬空时的平均速度(mm/us) 33 | //太小会导致轨迹失真，太大不能保证步态 34 | float flight_speed=0.0003; 35 | //轨迹参数 36 | //有两种轨迹可选，椭圆或两段直线 37 | struct trajy_para 38 | { 39 | float flight_period; //腿腾空的周期占比(1-占空比) 40 | uint8 trajy_type; //滞空的轨迹类型 1为椭圆轨迹，0为两段直线 41 | float tx; //腿的目标位置 42 | float tz; 43 | float hx; //抬腿的最高位置 44 | float hz; //如果是椭圆轨迹，表示椭圆半短轴 45 | uint8 num_talbe;//存入的轨迹表 46 | 47 | }; 48 | ``` 49 | - 调用或切换步态的流程 50 | ```C 51 | //相位 52 | phase[4]={0,0,0.5,0.5}; 53 | //生成轨迹存入轨迹表 54 | trajy_para param={0.5,1,40,0,20,20,0}; 55 | trajy_planning(param); 56 | //每条腿通过三段直线轨迹由当前位置过渡到新步态的初始位置 57 | //两个参数分别是过渡时的步态(0:trot,1:walk)和抬腿高度 58 | gait_transit(0,20); 59 | 60 | void loop() 61 | { 62 | //循环执行轨迹表 63 | gait_loop(); 64 | } 65 | 66 | 67 | ``` -------------------------------------------------------------------------------- /puppy_bot/.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < https://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < https://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < https://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choose one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | # language: python 28 | # python: 29 | # - "2.7" 30 | # 31 | # sudo: false 32 | # cache: 33 | # directories: 34 | # - "~/.platformio" 35 | # 36 | # install: 37 | # - pip install -U platformio 38 | # - platformio update 39 | # 40 | # script: 41 | # - platformio run 42 | 43 | 44 | # 45 | # Template #2: The project is intended to be used as a library with examples. 46 | # 47 | 48 | # language: python 49 | # python: 50 | # - "2.7" 51 | # 52 | # sudo: false 53 | # cache: 54 | # directories: 55 | # - "~/.platformio" 56 | # 57 | # env: 58 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 59 | # - PLATFORMIO_CI_SRC=examples/file.ino 60 | # - PLATFORMIO_CI_SRC=path/to/test/directory 61 | # 62 | # install: 63 | # - pip install -U platformio 64 | # - platformio update 65 | # 66 | # script: 67 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 68 | -------------------------------------------------------------------------------- /puppy_bot/src/kine.cpp: -------------------------------------------------------------------------------- 1 | #include "kine.h" 2 | 3 | 4 | 5 | void KINE::servo_attach(byte pin_thigh,byte pin_shank) 6 | { 7 | servo0=pin_thigh; 8 | servo1=pin_shank; 9 | 10 | } 11 | 12 | void KINE::mov(float vx,float vz) 13 | { 14 | //增量运动 15 | cx+=vx; 16 | cz+=vz; 17 | IK(); 18 | set_joint_rad(servo0,rad0); 19 | set_joint_rad(servo1,rad1); 20 | 21 | 22 | } 23 | 24 | 25 | void KINE::set_pose(float rx,float rz) 26 | { 27 | cx=rx; 28 | cz=rz; 29 | 30 | IK(); 31 | set_joint_rad(servo0,rad0); 32 | set_joint_rad(servo1,rad1); 33 | } 34 | 35 | 36 | void KINE::line_move(float rx,float rz) 37 | { 38 | //1mm一个插补点 39 | //float seg=1; 40 | float distance=sqrt(rx*rx+rz*rz); 41 | float seg_x=rx/distance; 42 | float seg_z=rz/distance; 43 | for(int i=0;i 0) 19 | { 20 | c = Serial.read(); 21 | if (c != cm_end) 22 | { 23 | command[command_length] = c; 24 | command_length++; 25 | } 26 | } 27 | 28 | if (command_length && (c == cm_end) ) 29 | { 30 | process_command(); 31 | init_command(); 32 | } 33 | 34 | } 35 | 36 | 37 | 38 | /*-----------------------------------------------------------------------------------------*/ 39 | void process_command() 40 | { 41 | 42 | byte code = 0; 43 | 44 | 45 | if (command_seen(cm_start) ) 46 | { 47 | 48 | code = (int)key_value(cm_start); 49 | switch (code) 50 | { 51 | 52 | case 0: 53 | pwmWrite(servo_num[int(key_value('n'))],int(key_value('p'))); 54 | Serial.println("done"); 55 | break; 56 | 57 | } 58 | 59 | } 60 | 61 | 62 | 63 | 64 | 65 | } 66 | /*-----------------------------------------------------------------------------------------*/ 67 | 68 | 69 | double key_value(char key) 70 | { 71 | char temp[10] = ""; 72 | 73 | for (byte i=0; i=period_points) 129 | ps[j]-=period_points; 130 | leg[j].set_pose(trajy_table[leg[j].table][ps[j]][X]+leg[j].spx,trajy_table[leg[j].table][ps[j]][Z]+leg[j].spz); 131 | 132 | } 133 | 134 | //在支撑相调整机器人(测试中) 135 | /* 136 | if((i>0.25*period_points&&i<0.5*period_points)||(i>0.75*period_points&&i