Skip to content

Commit 88316fb

Browse files
col-in-codingcol-in-coding
committed
new project for non-periodic gait test
1 parent bdb024e commit 88316fb

File tree

3 files changed

+295
-0
lines changed

3 files changed

+295
-0
lines changed

‎hardware_test/LabRobot/StableGait/QuadrupedRobot.cpp

Lines changed: 200 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,200 @@
1+
2+
#include <Arduino.h>
3+
#include "QuadrupedRobot.h"
4+
5+
/**
6+
* Switch on the robot
7+
*/
8+
void QuadrupedRobot::switch_on()
9+
{
10+
Serial.println("Robot Switch On");
11+
setup_servos();
12+
bot_rest();
13+
delay(2000);
14+
shut_servos();
15+
clear_cmd();
16+
}
17+
18+
/**
19+
* Switch off the robot
20+
*/
21+
void QuadrupedRobot::switch_off()
22+
{
23+
Serial.println("Robot Switch Off");
24+
shut_servos();
25+
clear_cmd();
26+
}
27+
28+
void QuadrupedRobot::clear_cmd()
29+
{
30+
command_code = -1;
31+
}
32+
33+
void QuadrupedRobot::setup_servos()
34+
{
35+
for (size_t i = 0; i < DOF; i++)
36+
{
37+
if (servos_ptr[i] == nullptr)
38+
servos_ptr[i] = new Servo();
39+
servos_ptr[i]->attach(servo_pins[i], SERVOMIN, SERVOMAX);
40+
}
41+
}
42+
43+
void QuadrupedRobot::shut_servos()
44+
{
45+
for (size_t i = 0; i < DOF; i++)
46+
{
47+
if (servos_ptr[i] != nullptr)
48+
{
49+
servos_ptr[i]->detach();
50+
}
51+
}
52+
}
53+
54+
/**
55+
* Check Interrupts
56+
* New Command from Serial Port
57+
*/
58+
bool QuadrupedRobot::interrupt()
59+
{
60+
String serial_in_str {""};
61+
while(Serial.available() > 0) {
62+
char inChar = Serial.read();
63+
serial_in_str += (char)inChar;
64+
// wait for input buffer read
65+
delay(10);
66+
}
67+
if(serial_in_str != "")
68+
{
69+
Serial.print("Command received: ");
70+
Serial.println(serial_in_str);
71+
command_code = serial_in_str.toInt();
72+
}
73+
}
74+
75+
void QuadrupedRobot::parse_code_run()
76+
{
77+
switch(command_code) {
78+
case 1:
79+
switch_on();
80+
break;
81+
case 2:
82+
bot_stand();
83+
break;
84+
case 3:
85+
bot_walk();
86+
break;
87+
case 0:
88+
switch_off();
89+
break;
90+
case 998:
91+
adjust();
92+
break;
93+
default:
94+
;// doing nothing
95+
}
96+
}
97+
98+
/**
99+
* Synchronous Servo Move
100+
* Every servo should move to the desired position at the end of the timestep
101+
*/
102+
void QuadrupedRobot::servo_move(float *angs_ptr)
103+
{
104+
unsigned long starttime = millis();
105+
unsigned long timenow = starttime;
106+
unsigned long servo_time[DOF] {}; // Timestamp of servo
107+
float joint_angs_now[DOF] {};
108+
float joint_degs_speed[DOF] {};
109+
for (size_t i = 0; i < DOF; i++)
110+
{
111+
joint_degs_speed[i] = (angs_ptr[i] - joint_angs_pre[i]) / timestep;
112+
joint_angs_now[i] = joint_angs_pre[i]; // Reset Joint angles
113+
servo_time[i] = starttime; // Reset Servo Time
114+
}
115+
while((timenow - starttime) < timestep) {
116+
for (size_t i = 0; i < DOF; i++)
117+
{
118+
timenow = millis();
119+
joint_angs_now[i] += joint_degs_speed[i] * (timenow - servo_time[i]);
120+
}
121+
servo_write_angs(joint_angs_now);
122+
timenow = millis();
123+
}
124+
// last run to assure that all servo has been desired position
125+
for (size_t i = 0; i < DOF; i++)
126+
{
127+
servo_write_angs(angs_ptr);
128+
}
129+
130+
131+
}
132+
133+
/**
134+
* Servo Write Current Angles
135+
* Current Angles is in Callibrated Coordinate,
136+
* which should be transfered to real angle degrees before using.
137+
*/
138+
void QuadrupedRobot::servo_write_angs(float *angs_ptr)
139+
{
140+
float real_ang{};
141+
float pulsewidth{};
142+
for (size_t i = 0; i < DOF; i++)
143+
{
144+
real_ang = init_servo_deg[i] + servo_dir[i] * angs_ptr[i];
145+
pulsewidth = 500 + real_ang / 90.0 * 1000;
146+
if (!servos_ptr[i]->attached())
147+
{
148+
servos_ptr[i]->attach(servo_pins[i], SERVOMIN, SERVOMAX);
149+
}
150+
servos_ptr[i]->writeMicroseconds(pulsewidth);
151+
// update current angles buffer
152+
joint_angs_pre[i] = angs_ptr[i];
153+
}
154+
}
155+
156+
/**
157+
* Only used for adjusting legs
158+
*/
159+
void QuadrupedRobot::adjust()
160+
{
161+
servo_write_angs(adjust_angs);
162+
clear_cmd();
163+
}
164+
165+
/**
166+
* Stand up
167+
* Non-periodic
168+
*/
169+
void QuadrupedRobot::bot_stand()
170+
{
171+
servo_write_angs(stand_angs);
172+
clear_cmd();
173+
}
174+
175+
/**
176+
* Sleep mode
177+
* Non-periodic
178+
*/
179+
void QuadrupedRobot::bot_rest()
180+
{
181+
servo_write_angs(rest_angs);
182+
clear_cmd();
183+
}
184+
185+
/**
186+
* walk
187+
* Periodic
188+
*/
189+
void QuadrupedRobot::bot_walk()
190+
{
191+
Serial.print("walk");
192+
}
193+
194+
QuadrupedRobot::~QuadrupedRobot()
195+
{
196+
for (size_t i = 0; i < DOF; i++)
197+
{
198+
delete servos_ptr[i];
199+
}
200+
}

‎hardware_test/LabRobot/StableGait/QuadrupedRobot.h

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
#ifndef QUADRUPED_ROBOT_H
2+
#define QUADRUPED_ROBOT_H
3+
4+
#include <Arduino.h>
5+
#include <Servo.h>
6+
7+
#define PT(s) Serial.print(s)
8+
#define PTL(s) Serial.println(s)
9+
#define PTF(s) Serial.print(F(s)) //trade flash memory for dynamic memory with F() function
10+
#define PTLF(s) Serial.println(F(s))
11+
12+
//servo constants
13+
#define SERVOMIN 500
14+
#define SERVOMAX 2500
15+
16+
#define DOF 12
17+
18+
class QuadrupedRobot
19+
{
20+
private:
21+
// For any non-periodic command, set it to -1 in the end of the action
22+
int command_code{-1};
23+
Servo *servos_ptr[DOF]{
24+
nullptr, nullptr, nullptr,
25+
nullptr, nullptr, nullptr,
26+
nullptr, nullptr, nullptr,
27+
nullptr, nullptr, nullptr};
28+
// HipZ, HipX, Knee
29+
byte servo_pins[DOF]{
30+
3, 4, 5,
31+
6, 7, 8,
32+
9, 10, 11,
33+
12, 13, 14};
34+
// Time taken by each sequence, used in servo_write_angs()
35+
unsigned long timestep = 500;
36+
37+
// Servo rotation configs
38+
const float init_servo_deg[12]{35, 90, 180, 130, 83, 0, 35, 85, 180, 130, 83, 0};
39+
const int8_t servo_dir[12]{1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1};
40+
41+
// float joint_angs_now[12]{}; //Current joint angles, in degrees
42+
float joint_angs_pre[12]{}; //Previous joint angles
43+
44+
// Angles in callibrated coordinate
45+
const float stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
46+
const float rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130};
47+
const float adjust_angs[12] {};
48+
49+
void setup_servos();
50+
void shut_servos();
51+
void servo_move(float *angs_ptr);
52+
void servo_write_angs(float *angs_ptr);
53+
void clear_cmd();
54+
55+
public:
56+
~QuadrupedRobot();
57+
bool interrupt();
58+
void parse_code_run();
59+
60+
void switch_on();
61+
void switch_off();
62+
63+
void adjust();
64+
void bot_rest();
65+
void bot_stand();
66+
void bot_walk();
67+
};
68+
69+
#endif

‎hardware_test/LabRobot/StableGait/StableGait.ino

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include "QuadrupedRobot.h"
2+
3+
QuadrupedRobot Robot;
4+
5+
void setup() {
6+
Serial.begin(9600);
7+
Serial.setTimeout(10);
8+
while (!Serial);
9+
// wait for ready
10+
while (Serial.available() && Serial.read()); // empty buffer
11+
delay(1000);
12+
13+
Serial.print(
14+
"Please select from menu: \n"
15+
" 1. switch on\n"
16+
" 2. stand\n"
17+
" 3. walk\n"
18+
" 0. switch off\n"
19+
" 998. adjust angles\n"
20+
);
21+
}
22+
23+
void loop() {
24+
Robot.interrupt();
25+
Robot.parse_code_run();
26+
}

0 commit comments

Comments
 (0)