└── main ├── Quadruped.cpp ├── Quadruped.h └── main.ino /main/Quadruped.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "Quadruped.h" 5 | 6 | Quadruped::Quadruped(float _w, float _l, float _l1, float _l2, float _l3){ 7 | W = _w; 8 | L = _l; 9 | L1 = _l1; 10 | L2 = _l2; 11 | L3 = _l3; 12 | 13 | init_local_frames(); 14 | update_frames_wrt_cog(); 15 | } 16 | 17 | void Quadruped::init_local_frames(){ 18 | local_frames.rf.x = 14.899; 19 | local_frames.rf.y = -60; 20 | local_frames.rf.z = 0; 21 | local_frames.lf.x = -14.899; 22 | local_frames.lf.y = -60; 23 | local_frames.lf.z = 0; 24 | local_frames.rb.x = 14.899; 25 | local_frames.rb.y = -60; 26 | local_frames.rb.z = 0; 27 | local_frames.lb.x = -14.899; 28 | local_frames.lb.y = -60; 29 | local_frames.lb.z = 0; 30 | } 31 | 32 | void Quadruped::attach_servos(){ 33 | rfh1.attach(servo_pins[0]); 34 | rfh2.attach(servo_pins[1]); 35 | rfk.attach(servo_pins[2]); 36 | lfh1.attach(servo_pins[3]); 37 | lfh2.attach(servo_pins[4]); 38 | lfk.attach(servo_pins[5]); 39 | rbh1.attach(servo_pins[6]); 40 | rbh2.attach(servo_pins[7]); 41 | rbk.attach(servo_pins[8]); 42 | lbh1.attach(servo_pins[9]); 43 | lbh2.attach(servo_pins[10]); 44 | lbk.attach(servo_pins[11]); 45 | } 46 | 47 | void Quadruped::init_initialise(){ 48 | attach_servos(); 49 | 50 | rfh1.write(zero_positions[0]); 51 | rfh2.write(zero_positions[1]); 52 | rfk.write(zero_positions[2]); 53 | lfh1.write(zero_positions[3]); 54 | lfh2.write(zero_positions[4]); 55 | lfk.write(zero_positions[5]); 56 | rbh1.write(zero_positions[6]); 57 | rbh2.write(zero_positions[7]); 58 | rbk.write(zero_positions[8]); 59 | lbh1.write(zero_positions[9]); 60 | lbh2.write(zero_positions[10]); 61 | lbk.write(zero_positions[11]); 62 | 63 | delay(1000); 64 | } 65 | 66 | void Quadruped::initialise(){ 67 | IK(); 68 | attach_servos(); 69 | delay(1000); 70 | } 71 | 72 | void Quadruped::IK(){ 73 | float theta1rf = -atan2(-local_frames.rf.y, local_frames.rf.x)-atan2(sqrt(sq(local_frames.rf.x)+sq(local_frames.rf.y)-sq(L1)), -L1); 74 | float Drf = (sq(local_frames.rf.x)+sq(local_frames.rf.y)-sq(L1)+sq(-local_frames.rf.z)-sq(L2)-sq(L3))/(2*L2*L3); 75 | float theta3rf = atan2(-sqrt(1-sq(Drf)), Drf); 76 | float theta2rf = atan2(-local_frames.rf.z, sqrt(sq(local_frames.rf.x)+sq(local_frames.rf.y)-sq(L1)))-atan2(L3 * sin(theta3rf), L2+L3*cos(theta3rf)); 77 | float theta1rf_deg = (180.0/M_PI)*theta1rf; 78 | float theta2rf_deg = (180.0/M_PI)*theta2rf; 79 | float theta3rf_deg = (180.0/M_PI)*theta3rf; 80 | 81 | float theta1lf = -atan2(-local_frames.lf.y, -local_frames.lf.x)-atan2(sqrt(sq(-local_frames.lf.x)+sq(local_frames.lf.y)-sq(L1)), -L1); 82 | float Dlf = (sq(-local_frames.lf.x)+sq(local_frames.lf.y)-sq(L1)+sq(-local_frames.lf.z)-sq(L2)-sq(L3))/(2*L2*L3); 83 | float theta3lf = atan2(-sqrt(1-sq(Dlf)), Dlf); 84 | float theta2lf = atan2(-local_frames.lf.z, sqrt(sq(-local_frames.lf.x)+sq(local_frames.lf.y)-sq(L1)))-atan2(L3 * sin(theta3lf), L2+L3*cos(theta3lf)); 85 | float theta1lf_deg = (180.0/M_PI)*theta1lf; 86 | float theta2lf_deg = (180.0/M_PI)*theta2lf; 87 | float theta3lf_deg = (180.0/M_PI)*theta3lf; 88 | 89 | float theta1rb = -atan2(-local_frames.rb.y, local_frames.rb.x)-atan2(sqrt(sq(local_frames.rb.x)+sq(local_frames.rb.y)-sq(L1)), -L1); 90 | float Drb = (sq(local_frames.rb.x)+sq(local_frames.rb.y)-sq(L1)+sq(local_frames.rb.z)-sq(L2)-sq(L3))/(2*L2*L3); 91 | float theta3rb = atan2(-sqrt(1-sq(Drb)), Drb); 92 | float theta2rb = atan2(local_frames.rb.z, sqrt(sq(local_frames.rb.x)+sq(local_frames.rb.y)-sq(L1)))-atan2(L3 * sin(theta3rb), L2+L3*cos(theta3rb)); 93 | float theta1rb_deg = (180.0/M_PI)*theta1rb; 94 | float theta2rb_deg = (180.0/M_PI)*theta2rb; 95 | float theta3rb_deg = (180.0/M_PI)*theta3rb; 96 | 97 | float theta1lb = -atan2(-local_frames.lb.y, -local_frames.lb.x)-atan2(sqrt(sq(-local_frames.lb.x)+sq(local_frames.lb.y)-sq(L1)), -L1); 98 | float Dlb = (sq(-local_frames.lb.x)+sq(local_frames.lb.y)-sq(L1)+sq(local_frames.lb.z)-sq(L2)-sq(L3))/(2*L2*L3); 99 | float theta3lb = atan2(-sqrt(1-sq(Dlb)), Dlb); 100 | float theta2lb = atan2(local_frames.lb.z, sqrt(sq(-local_frames.lb.x)+sq(local_frames.lb.y)-sq(L1)))-atan2(L3 * sin(theta3lb), L2+L3*cos(theta3lb)); 101 | float theta1lb_deg = (180.0/M_PI)*theta1lb; 102 | float theta2lb_deg = (180.0/M_PI)*theta2lb; 103 | float theta3lb_deg = (180.0/M_PI)*theta3lb; 104 | 105 | rfh1.write(zero_positions[0] + (-180 - theta1rf_deg)); 106 | rfh2.write(zero_positions[1] - theta2rf_deg); 107 | rfk.write(zero_positions[2] + theta3rf_deg); 108 | 109 | lfh1.write(zero_positions[3] - (-180 - theta1lf_deg)); 110 | lfh2.write(zero_positions[4] + theta2lf_deg); 111 | lfk.write(zero_positions[5] - theta3lf_deg); 112 | 113 | rbh1.write(zero_positions[6] - (-180 - theta1rb_deg)); 114 | rbh2.write(zero_positions[7] + theta2rb_deg); 115 | rbk.write(zero_positions[8] - theta3rb_deg); 116 | 117 | lbh1.write(zero_positions[9] + (-180 - theta1lb_deg)); 118 | lbh2.write(zero_positions[10] - theta2lb_deg); 119 | lbk.write(zero_positions[11] + theta3lb_deg); 120 | 121 | // Serial.println("finshed moving"); 122 | } 123 | 124 | void Quadruped::IK_one_leg(String leg, float x, float y, float z){ 125 | if (leg == "rf"){ 126 | local_frames.rf.x += x; 127 | local_frames.rf.y += y; 128 | local_frames.rf.z += z; 129 | } 130 | else if (leg == "lf"){ 131 | local_frames.lf.x += x; 132 | local_frames.lf.y += y; 133 | local_frames.lf.z += z; 134 | } 135 | else if (leg == "rb"){ 136 | local_frames.rb.x += x; 137 | local_frames.rb.y += y; 138 | local_frames.rb.z += z; 139 | } 140 | else if (leg == "lb"){ 141 | local_frames.lb.x += x; 142 | local_frames.lb.y += y; 143 | local_frames.lb.z += z; 144 | } 145 | IK(); 146 | } 147 | 148 | void Quadruped::update_frames_wrt_cog(){ 149 | frames_wrt_cog.rf.x = local_frames.rf.x + W/2; 150 | frames_wrt_cog.rf.y = local_frames.rf.y; 151 | frames_wrt_cog.rf.z = local_frames.rf.z + L/2; 152 | 153 | frames_wrt_cog.lf.x = local_frames.lf.x - W/2; 154 | frames_wrt_cog.lf.y = local_frames.lf.y; 155 | frames_wrt_cog.lf.z = local_frames.lf.z + L/2; 156 | 157 | frames_wrt_cog.rb.x = local_frames.rb.x + W/2; 158 | frames_wrt_cog.rb.y = local_frames.rb.y; 159 | frames_wrt_cog.rb.z = local_frames.rb.z - L/2; 160 | 161 | frames_wrt_cog.lb.x = local_frames.lb.x - W/2; 162 | frames_wrt_cog.lb.y = local_frames.lb.y; 163 | frames_wrt_cog.lb.z = local_frames.lb.z - L/2; 164 | } 165 | 166 | void Quadruped::update_local_frames(){ 167 | local_frames.rf.x = frames_wrt_cog.rf.x - W/2; 168 | local_frames.rf.y = frames_wrt_cog.rf.y; 169 | local_frames.rf.z = frames_wrt_cog.rf.z - L/2; 170 | 171 | local_frames.lf.x = frames_wrt_cog.lf.x + W/2; 172 | local_frames.lf.y = frames_wrt_cog.lf.y; 173 | local_frames.lf.z = frames_wrt_cog.lf.z - L/2; 174 | 175 | local_frames.rb.x = frames_wrt_cog.rb.x - W/2; 176 | local_frames.rb.y = frames_wrt_cog.rb.y; 177 | local_frames.rb.z = frames_wrt_cog.rb.z + L/2; 178 | 179 | local_frames.lb.x = frames_wrt_cog.lb.x + W/2; 180 | local_frames.lb.y = frames_wrt_cog.lb.y; 181 | local_frames.lb.z = frames_wrt_cog.lb.z + L/2; 182 | } 183 | 184 | void Quadruped::translate_cog(float x, float z){ 185 | cog_x += x; 186 | cog_z += z; 187 | 188 | Serial.print(cog_x); 189 | Serial.print(", "); 190 | Serial.println(cog_z); 191 | 192 | local_frames.rf.x -= x; 193 | local_frames.lf.x -= x; 194 | local_frames.rb.x -= x; 195 | local_frames.lb.x -= x; 196 | 197 | local_frames.rf.z -= z; 198 | local_frames.lf.z -= z; 199 | local_frames.rb.z -= z; 200 | local_frames.lb.z -= z; 201 | 202 | IK(); 203 | } 204 | 205 | void Quadruped::translate_cog_on_support_polygon(String swing_leg){ 206 | float support_polygon_centroid_x; 207 | float support_polygon_centroid_z; 208 | 209 | update_frames_wrt_cog(); 210 | 211 | // Serial.print("("); 212 | // Serial.print(frames_wrt_cog.rf.x); 213 | // Serial.print(", "); 214 | // Serial.print(frames_wrt_cog.rf.z); 215 | // Serial.println(")"); 216 | // 217 | // Serial.print("("); 218 | // Serial.print(frames_wrt_cog.lf.x); 219 | // Serial.print(", "); 220 | // Serial.print(frames_wrt_cog.lf.z); 221 | // Serial.println(")"); 222 | // 223 | // Serial.print("("); 224 | // Serial.print(frames_wrt_cog.rb.x); 225 | // Serial.print(", "); 226 | // Serial.print(frames_wrt_cog.rb.z); 227 | // Serial.println(")"); 228 | // 229 | // Serial.print("("); 230 | // Serial.print(frames_wrt_cog.lb.x); 231 | // Serial.print(", "); 232 | // Serial.print(frames_wrt_cog.lb.z); 233 | // Serial.println(")"); 234 | 235 | 236 | if (swing_leg == "rf"){ 237 | support_polygon_centroid_x = (frames_wrt_cog.lf.x + frames_wrt_cog.rb.x + frames_wrt_cog.lb.x) / 3.0; 238 | support_polygon_centroid_z = (frames_wrt_cog.lf.z + frames_wrt_cog.rb.z + frames_wrt_cog.lb.z) / 3.0; 239 | } 240 | else if (swing_leg == "lf"){ 241 | support_polygon_centroid_x = (frames_wrt_cog.rf.x + frames_wrt_cog.rb.x + frames_wrt_cog.lb.x) / 3.0; 242 | support_polygon_centroid_z = (frames_wrt_cog.rf.z + frames_wrt_cog.rb.z + frames_wrt_cog.lb.z) / 3.0; 243 | } 244 | else if (swing_leg == "rb"){ 245 | support_polygon_centroid_x = (frames_wrt_cog.rf.x + frames_wrt_cog.lf.x + frames_wrt_cog.lb.x) / 3.0; 246 | support_polygon_centroid_z = (frames_wrt_cog.rf.z + frames_wrt_cog.lf.z + frames_wrt_cog.lb.z) / 3.0; 247 | } 248 | else if (swing_leg == "lb"){ 249 | support_polygon_centroid_x = (frames_wrt_cog.rf.x + frames_wrt_cog.lf.x + frames_wrt_cog.rb.x) / 3.0; 250 | support_polygon_centroid_z = (frames_wrt_cog.rf.z + frames_wrt_cog.lf.z + frames_wrt_cog.rb.z) / 3.0; 251 | } 252 | 253 | // Serial.println("Support polygon centroid"); 254 | // Serial.print("("); 255 | // Serial.print(support_polygon_centroid_x); 256 | // Serial.print(", "); 257 | // Serial.print(support_polygon_centroid_z); 258 | // Serial.println(")"); 259 | 260 | float old_x = 0; 261 | float old_z = 0; 262 | for (int i = 0; i<=100; i++){ 263 | float new_x = i * support_polygon_centroid_x / 100.0; 264 | float new_z = i * support_polygon_centroid_z / 100.0; 265 | translate_cog(new_x - old_x, new_z - old_z); 266 | old_x = new_x; 267 | old_z = new_z; 268 | delay(1); 269 | } 270 | 271 | } 272 | -------------------------------------------------------------------------------- /main/Quadruped.h: -------------------------------------------------------------------------------- 1 | #ifndef Quadruped_h 2 | #define Quadruped_h 3 | 4 | #include 5 | 6 | typedef struct frame{ 7 | float x; 8 | float y; 9 | float z; 10 | }Frame; 11 | 12 | typedef struct frames{ 13 | Frame rf; 14 | Frame lf; 15 | Frame rb; 16 | Frame lb; 17 | }Frames; 18 | 19 | class Quadruped{ 20 | private: 21 | float W; 22 | float L; 23 | 24 | float L1; 25 | float L2; 26 | float L3; 27 | 28 | float cog_x = 0.0; 29 | float cog_z = 0.0; 30 | float cog_y = 60.0; 31 | 32 | int servo_pins[12] = {4, 3, 2, 7, 6, 5, 10, 8, 9, 13, 12, 11}; 33 | int zero_positions[12] = {83, 89, 164, 85, 93, 3, 102, 90, 15, 90, 99, 175}; 34 | 35 | Servo rfh1, rfh2, rfk, lfh1, lfh2, lfk, rbh1, rbh2, rbk, lbh1, lbh2, lbk; 36 | 37 | void init_local_frames(); 38 | void attach_servos(); 39 | 40 | public: 41 | Quadruped(float _w, float _l, float _l1, float _l2, float _l3); 42 | 43 | Frames local_frames; 44 | Frames frames_wrt_cog; 45 | 46 | void init_initialise(); 47 | void initialise(); 48 | void IK(); 49 | void IK_one_leg(String leg, float x, float y, float z); 50 | void update_frames_wrt_cog(); 51 | void update_local_frames(); 52 | void translate_cog(float x, float y); 53 | void translate_cog_on_support_polygon(String swing_leg); 54 | }; 55 | 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /main/main.ino: -------------------------------------------------------------------------------- 1 | #include "Quadruped.h" 2 | 3 | Quadruped robot(48.302, 91, 15.151, 40, 40); 4 | 5 | void setup() { 6 | // put your setup code here, to run once: 7 | Serial.begin(9600); 8 | // robot.init_initialise(); 9 | robot.initialise(); 10 | 11 | delay(100); 12 | robot.translate_cog_on_support_polygon("rf"); 13 | // delay(1000); 14 | robot.IK_one_leg("rf", 0,20,10); 15 | delay(100); 16 | robot.IK_one_leg("rf", 0,-20,5); 17 | 18 | delay(100); 19 | robot.translate_cog_on_support_polygon("lb"); 20 | // delay(1000); 21 | robot.IK_one_leg("lb", 0,20,10); 22 | delay(100); 23 | robot.IK_one_leg("lb", 0,-20,5); 24 | 25 | delay(100); 26 | robot.translate_cog_on_support_polygon("lf"); 27 | // delay(1000); 28 | robot.IK_one_leg("lf", 0,20,20); 29 | delay(100); 30 | robot.IK_one_leg("lf", 0,-20,10); 31 | 32 | delay(100); 33 | robot.translate_cog_on_support_polygon("rb"); 34 | // delay(1000); 35 | robot.IK_one_leg("rb", 0,20,20); 36 | delay(100); 37 | robot.IK_one_leg("rb", 0,-20,10); 38 | } 39 | 40 | void loop() { 41 | // put your main code here, to run repeatedly: 42 | delay(100); 43 | robot.translate_cog_on_support_polygon("rf"); 44 | // delay(1000); 45 | robot.IK_one_leg("rf", 0,20,20); 46 | delay(100); 47 | robot.IK_one_leg("rf", 0,-20,10); 48 | 49 | delay(100); 50 | robot.translate_cog_on_support_polygon("lb"); 51 | // delay(1000); 52 | robot.IK_one_leg("lb", 0,20,20); 53 | delay(100); 54 | robot.IK_one_leg("lb", 0,-20,10); 55 | 56 | delay(100); 57 | robot.translate_cog_on_support_polygon("lf"); 58 | // delay(1000); 59 | robot.IK_one_leg("lf", 0,20,20); 60 | delay(100); 61 | robot.IK_one_leg("lf", 0,-20,10); 62 | 63 | delay(100); 64 | robot.translate_cog_on_support_polygon("rb"); 65 | // delay(1000); 66 | robot.IK_one_leg("rb", 0,20,20); 67 | delay(100); 68 | robot.IK_one_leg("rb", 0,-20,10); 69 | } 70 | --------------------------------------------------------------------------------