Skip to content

Commit

Permalink
corrected all
Browse files Browse the repository at this point in the history
  • Loading branch information
@kpaonaut
kpaonaut committed Dec 1, 2017
1 parent af0f274 commit 390510b
Show file tree
Hide file tree
Showing 8 changed files with 100 additions and 95 deletions.
31 changes: 18 additions & 13 deletions STM32Project/hip.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,25 +7,25 @@
//using namespace std; //debug

void StepHipTraj::reset() {
t_ = 0;
t_ = 0;
numPiece = 0;
n = 4;
}

int StepHipTraj::Increment(float* traj_value ) {
//*traj_value = max_hip_flexion_ * sin(t_ * 0.001); // Sample calculation
//*traj_value = max_hip_flexion_ * sin(t_ * 0.001); // Sample calculation

if (numPiece < n-1 && t_ > X[numPiece+1]) numPiece++;

*traj_value = s(t_) + walking_angle_;

if (t_ >= 2*step_time_) {
if (t_ >= 2*step_time_) {
t_ -= 2*step_time_;
return 1;
}else{
t_ ++; // Incrementing current time by one time unit
return 0;
}
return 1;
}else{
t_ ++; // Incrementing current time by one time unit
return 0;
}
}

#define PI 3.14159265
Expand All @@ -38,7 +38,8 @@ void StepHipTraj::init() // the generation of spline
x[3] = 2*step_time_;
y[0] = swing_start_;
y[1] = max_hip_flexion_;
y[2] = 2 * asin(step_range_ / 2.0 / leglen) * 180 / PI;
//y[2] = 2 * asin(step_range_ / 2.0 / leglen) * 180 / PI;
y[2] = second_hip_flexion_;
y[3] = swing_start_;

splineInterpolate();
Expand Down Expand Up @@ -66,20 +67,24 @@ void StepHipTraj::set_max_hip_flexion(float value) {
max_hip_flexion_ = value;
}

void StepHipTraj::set_second_hip_flexion(float value) {
second_hip_flexion_ = value;
}

void StepHipTraj::set_max_hip_flexion_time(float value) {
max_hip_flexion_time_ = value; // in ms
}

void StepHipTraj::set_walking_angle(float value) {
walking_angle_ = value;
walking_angle_ = value;
}

void StepHipTraj::set_swing_start(float value) {
swing_start_ = value;
swing_start_ = value;
}

void StepHipTraj::set_step_time(float value) {
step_time_ = value; // in ms
step_time_ = value; // in ms
}

void StepHipTraj::set_step_range(float value) {
Expand All @@ -88,4 +93,4 @@ void StepHipTraj::set_step_range(float value) {

void StepHipTraj::set_leg_length(float value) {
leglen = value;
}
}
33 changes: 18 additions & 15 deletions STM32Project/hip.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,22 +9,23 @@

class StepHipTraj {
public:
StepHipTraj() {
}
void reset();
int Increment(float* traj_value);
void set_max_hip_flexion(float value);
StepHipTraj() {
}
void reset();
int Increment(float* traj_value);
void set_max_hip_flexion(float value);
void set_second_hip_flexion(float value);
void set_max_hip_flexion_time(float value);
void set_walking_angle(float value); // offest for the entire trajectory
void set_swing_start(float value);
void set_step_time(float value);
void set_walking_angle(float value); // offest for the entire trajectory
void set_swing_start(float value);
void set_step_time(float value);
void set_step_range(float value); // step length, in meter
void set_leg_length(float);
void init();
void splineInterpolate();

inline int get_step_time(){
return step_time_;
inline int get_step_time(){
return step_time_;
}

inline float get_time(){
Expand All @@ -36,14 +37,15 @@ class StepHipTraj {
}

private:
float swing_start_;
float max_hip_flexion_;
float swing_start_;
float max_hip_flexion_;
float second_hip_flexion_;
float max_hip_flexion_time_;
float walking_angle_;
float walking_angle_;
float step_range_; // in meter
float step_time_; // step time range, half the time of a cycle
float step_time_; // step time range, half the time of a cycle
float t_;
int numPiece, n; // which piece of spline the current point belongs to, starting from 0
int numPiece, n; // which piece of spline the current point belongs to, starting from 0
// n: the number of critical pts, for hip it's 4
float leglen;
float x[4];
Expand All @@ -54,3 +56,4 @@ class StepHipTraj {
};

#endif /* TRAJECTORY_TEMPLATE_H_ */

2 changes: 1 addition & 1 deletion STM32Project/knee.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ void StepKneeTraj::reset() {
n = 5;
}

int StepKneeTraj::Increment(float* traj_value ) {
float StepKneeTraj::Increment(float* traj_value ) {

if (t_ < (2*step_time_ + starting_time_)) // there is an x-offset as well!
*traj_value = s(t_); // no walking angle offset for knee relative angle
Expand Down
18 changes: 10 additions & 8 deletions STM32Project/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,20 @@ This file is what runs in the STM32 board system.
*/
#include "mbed.h"
# include "hip.h"
# include "knee.h"
# include "knee.h"
# include "trajectory_generator.h"

DigitalOut led1(LED1); // blink and debug
DigitalOut led2(LED2);
Serial pc(USBTX,USBRX,115200);

static float hipAngle = 0;
void periodic_function()
{
static int counter = 0;
counter++;
if((counter%1000)==0) {
led2=!led2;
pc.printf("Hip angle:%f\n", hipAngle);
}
}

Expand All @@ -23,22 +25,22 @@ int main()
{
pc.printf("main\n\r");

hipAngle = 0;
Ticker control_loop;
control_loop.attach(&periodic_function, 0.001);
bool stop = 0;
TrajectoryGenerator generator;
float hipAngle;
while (true) {
if (pc.readable()) {
char c = pc.getc();
pc.printf("User inserted: %c\n\r",c);
led1 = !led1;
stop = !stop;
if (stop)
generator.stopTrigger();
else
generator.resumeTrigger();
}
if (stop)
generator.stopTrigger();
else
generator.resumeTrigger();
hipAngle = generator.generateTrajectory();
}
}
}
1 change: 1 addition & 0 deletions STM32Project/parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
// The value commented are default values (tuned values)
namespace par{
#define _max_hip_flexion_ 36
#define _second_hip_flexion_ 25.3
#define _max_hip_flexion_time_portion_ 0.8
#define _hip_swing_start_ -12
#define _step_range_ 0.42
Expand Down
79 changes: 41 additions & 38 deletions STM32Project/trajectory_generator.cpp → STM32Project/trajectory_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,18 @@
#include <cstdio>
#include <cstdlib>
#include <vector>

// #include "mbed.h"
// Serial pc2(USBTX,USBRX,115200);
class TrajectoryGenerator{
private:
float t, walking_angle, sample_time, halfTotalTime;
int rightStarted, doneKnee, doneHip;
int rightStarted, doneKnee, doneHip, stopTriggered;
StepHipTraj hip_trajectory_generator_l, hip_trajectory_generator_r;
StepKneeTraj knee_trajectory_generator_l, knee_trajectory_generator_r;
float hip_angle, knee_angle;

public:
TrajectoryGenerator( t = 0, walking_angle = par::_walking_angle_, sample_time = 0.001, halfTotalTime = 1) // FIXME: the step time is set to be very small because plot is too slow
TrajectoryGenerator(float t = 0, float walking_angle = _walking_angle_, float sample_time = 0.001, float halfTotalTime = 1): // FIXME: the step time is set to be very small because plot is too slow
t(t),
walking_angle(walking_angle),
sample_time(sample_time), // in seconds
Expand All @@ -25,24 +28,25 @@ class TrajectoryGenerator{
hip_trajectory_generator_l = StepHipTraj(); // new object

hip_trajectory_generator_l.reset();
hip_trajectory_generator_l.set_max_hip_flexion( par::_max_hip_flexion_ ); // degrees, default 36
hip_trajectory_generator_l.set_max_hip_flexion_time( par::_max_hip_flexion_time_portion_ * halfTotalTime / sample_time); // in ms
hip_trajectory_generator_l.set_max_hip_flexion( _max_hip_flexion_ ); // degrees, default 36
hip_trajectory_generator_l.set_second_hip_flexion( _second_hip_flexion_ ); // degrees, default 36
hip_trajectory_generator_l.set_max_hip_flexion_time( _max_hip_flexion_time_portion_ * halfTotalTime / sample_time); // in ms
hip_trajectory_generator_l.set_step_time( halfTotalTime / sample_time ); // in ms, integer. 5000 here.
hip_trajectory_generator_l.set_walking_angle( walking_angle ); // offset, can vary
hip_trajectory_generator_l.set_swing_start( par::_hip_swing_start_ ); // default -6.934
hip_trajectory_generator_l.set_step_range( par::_step_range_ ); // step length in meter. example: 0.78m legLen 1.1m -> theta 41.8
hip_trajectory_generator_l.set_leg_length( par::_leg_length_ );
hip_trajectory_generator_l.set_swing_start( _hip_swing_start_ ); // default -6.934
hip_trajectory_generator_l.set_step_range( _step_range_ ); // step length in meter. example: 0.78m legLen 1.1m -> theta 41.8
hip_trajectory_generator_l.set_leg_length( _leg_length_ );

// left Knee
knee_trajectory_generator_l = StepKneeTraj(); // new object

knee_trajectory_generator_l.reset();
knee_trajectory_generator_l.set_max_hip_flexion_time( par::_max_hip_flexion_time_portion_*halfTotalTime/sample_time ); // in ms
knee_trajectory_generator_l.set_max_hip_flexion_time( _max_hip_flexion_time_portion_*halfTotalTime/sample_time ); // in ms
knee_trajectory_generator_l.set_step_time( halfTotalTime / sample_time ); // in ms, integer. 5000 here.
knee_trajectory_generator_l.set_max_knee_flexion( par::_max_knee_flexion_ ); // in degrees
knee_trajectory_generator_l.set_second_knee_flexion( par::_second_knee_flexion_ );
knee_trajectory_generator_l.set_min_knee_flexion( par::_min_knee_flexion_ );
knee_trajectory_generator_l.set_walking_angle( walking_angle );
knee_trajectory_generator_l.set_max_knee_flexion( _max_knee_flexion_ ); // in degrees
knee_trajectory_generator_l.set_second_knee_flexion( _second_knee_flexion_ );
knee_trajectory_generator_l.set_min_knee_flexion( _min_knee_flexion_ );
knee_trajectory_generator_l.set_walking_angle( walking_angle );

hip_trajectory_generator_l.init(); // set coordinates for interpolation
knee_trajectory_generator_l.init();
Expand All @@ -51,23 +55,23 @@ class TrajectoryGenerator{
hip_trajectory_generator_r = StepHipTraj(); // right leg
hip_trajectory_generator_r.reset();
hip_trajectory_generator_r.set_max_hip_flexion( par::_max_hip_flexion_ ); // degrees, default 36
hip_trajectory_generator_r.set_max_hip_flexion_time( par::_max_hip_flexion_time_portion_ * halfTotalTime/sample_time); // in ms
hip_trajectory_generator_r.set_max_hip_flexion( _max_hip_flexion_ ); // degrees, default 36
hip_trajectory_generator_r.set_max_hip_flexion_time( _max_hip_flexion_time_portion_ * halfTotalTime/sample_time); // in ms
hip_trajectory_generator_r.set_step_time( halfTotalTime / sample_time ); // in ms, integer. 5000 here.
hip_trajectory_generator_r.set_walking_angle( walking_angle ); // offset, can vary
hip_trajectory_generator_r.set_swing_start( par::_hip_swing_start_ ); // default -6.934
hip_trajectory_generator_r.set_step_range( par::_step_range_ ); // step length in meter. example: 0.78m legLen 1.1m -> theta 41.8
hip_trajectory_generator_r.set_leg_length( par::_leg_length_ );
hip_trajectory_generator_r.set_swing_start( _hip_swing_start_ ); // default -6.934
hip_trajectory_generator_r.set_step_range( _step_range_ ); // step length in meter. example: 0.78m legLen 1.1m -> theta 41.8
hip_trajectory_generator_r.set_leg_length( _leg_length_ );
// right Knee
knee_trajectory_generator_r = StepKneeTraj(); // new object
knee_trajectory_generator_r.reset();
knee_trajectory_generator_r.set_max_hip_flexion_time( par::_max_hip_flexion_time_portion_*halfTotalTime/sample_time ); // in ms
knee_trajectory_generator_r.set_max_hip_flexion_time( _max_hip_flexion_time_portion_*halfTotalTime/sample_time ); // in ms
knee_trajectory_generator_r.set_step_time( halfTotalTime/sample_time ); // in ms, integer. 5000 here.
knee_trajectory_generator_r.set_max_knee_flexion( par::_max_knee_flexion_ ); // in degrees
knee_trajectory_generator_r.set_second_knee_flexion( par::_second_knee_flexion_ );
knee_trajectory_generator_r.set_min_knee_flexion( par::_min_knee_flexion_ );
knee_trajectory_generator_r.set_max_knee_flexion( _max_knee_flexion_ ); // in degrees
knee_trajectory_generator_r.set_second_knee_flexion( _second_knee_flexion_ );
knee_trajectory_generator_r.set_min_knee_flexion( _min_knee_flexion_ );
knee_trajectory_generator_r.set_walking_angle( walking_angle );
hip_trajectory_generator_r.init() // set coordinates for interpolation
Expand All @@ -76,6 +80,8 @@ class TrajectoryGenerator{
doneHip = 0;
doneKnee = 0;
stopTriggered = 0;
hip_angle = 0; // will be altering with time
knee_angle = 0; //
}

inline void stopTrigger(){ // reserved interface for outside triggering!
Expand All @@ -87,39 +93,36 @@ class TrajectoryGenerator{
}

void carryOut(){ // carry out calculation
float hip_value_l = hip_trajectory_generator_l.Increment();
float knee_value_l = knee_trajectory_generator_l.Increment(); // generate traj in next time step
// two knees are stoped together, as their derivative happen to be 0 at the same time, one at minima, one at maxima
// two hips are stopeed the same way.
doneHip = hip_trajectory_generator_l.Increment(&hip_angle);
doneKnee = knee_trajectory_generator_l.Increment(&knee_angle); // generate traj in next time step
/* carry out the value for the other leg. No need in STM32!
if ((rightStarted == 1) || (hip_trajectory_generator_l.get_time() > halfTotalTime))
{
hip_value_r = hip_trajectory_generator_r.Increment();
knee_value_r = knee_trajectory_generator_r.Increment(); // generate traj in next time step
doneHip= hip_trajectory_generator_r.Increment(&hip_angle); // hip angle will be changed!
doneKnee = knee_trajectory_generator_r.Increment(); // generate traj in next time step
rightStarted = 1;
}
else
{
hip_value_r = hip_trajectory_generator_r.get_initial_pos();
knee_value_r = knee_trajectory_generator_r.get_initial_pos();
}

doneHip = doneHipl | doneHipr;
doneKnee = doneKneel | doneKneer;
}*/
}

float generateTrajectory(){
if (stopTriggered)
if (doneHip == 0)
carryOut();

else;
else // stop signal not triggered
carryOut();

angles = [walking_angle, hip_value_l - walking_angle, knee_value_l,
hip_value_r - walking_angle, knee_value_r];
// angles = [walking_angle, hip_value_l - walking_angle, knee_value_l,
// hip_value_r - walking_angle, knee_value_r];
// print hip_trajectory_generator_l.get_time(), hip_trajectory_generator_r.get_time()
return hip_value_l;
//pc.printf("%f!!!\n", hip_angle);
return hip_angle; // only return hip! When is knee needed?
}

inline float getTime() return hip_trajectory_generator_l.get_time();
};
inline float getTime() {return hip_trajectory_generator_l.get_time();}
};
Binary file added a
View file
Binary file not shown.
Loading

0 comments on commit 390510b

Please sign in to comment.