Bionic Robot Arduino

Programming, Tutorial, Coding, New Design, and/or any project for your Arduino and Raspberry Pi can post your topic /suggestion here.
User avatar
BotsSniper
Posts: 15
Joined: Tue Jul 05, 2016 1:43 pm

Bionic Robot Arduino

Thu Jul 21, 2016 9:41 am

Bionic Robot Arduino

Image

I'm giving out this code in order for you guys to try. Below is the Link. Please dont forget to share your output when you finished., or made some improvement with the code.

For live Sample link below,
https://www.youtube.com/watch?v=75_-ZaLE8i0

Image

bionic.ino

Code: Select all

/******************************************
  Project Date: 26 May 2015
  Barbe Pramis
  Wild Cat -Frexcy
  Inspired by: Boston Dynamics Design
  Prototype: Update 02
  Next Plan: 2.4Ghz Remote Control 
             or Java Scrift Communication,
             

*******************************************/

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include "includes.h"

//STATE VARIABLES
boolean MOVING = false;
boolean STOPPING = false;
int STEPS_IN_WALK = 4;
int CURRENT_WALK = LATERAL_WALK;
int CURRENT_POSE;
int NEXT_POSE;
int CURRENT_STEP = 0;
int GRANULARITY = 4; //10;4
int CURRENT_TICK = 0;
int STEP_COUNT = 0;
int WALK_CYCLE_COUNT = 0;
int TICK_DELAY = 50;
struct point CURRENT_TRAJECTORY[4][10];
boolean CHAT = false;

void setup() 
{
  Serial.begin(9600);
  Serial.println("FREXCY V.2");
  
  pwm.begin();
  pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
  
  for(int i=0;i<4;i++)
  {
	legs[i] = make_leg(i);
  }
 
  //STAND 
  //test_reverse_trajectory();
}

void loop() 
{
    communicating();
    moving();
}

void communicating()
{
   //if we have a complete message flush the package
   //so its ready for a new command
   if(message_complete) 
   {
      message_complete = false;
      message = "";   
   } 
}

void moving()
{
  //check if we are moving....
  if(!MOVING) return;
  walking();
}

void serialEvent() 
{
  int i=0;
  if (Serial.available()) 
  {
    delay(50);
    while(Serial.available()) {
      int inChar = Serial.read();
      message+= (char)inChar;
    }
    message_complete = true;
    command();
  }
}

void command()
{

  Serial.print("CMD:");
  Serial.println(message);
  
  
  if(message.startsWith("MV")) MVAction();
  if(message.startsWith("SP")) SPAction();
  if(message.startsWith("RX")) RXAction();
  if(message.startsWith("RY")) RYAction();
  if(message.startsWith("OX")) OXAction();
  if(message.startsWith("OY")) OYAction();

}

void OXAction()
{
   int leg_idx = message.substring(2,3).toInt();
   int val = message.substring(3).toInt();
   legs[leg_idx -1].flight.origin.x = val;
}

void OYAction()
{
    int leg_idx = message.substring(2,3).toInt();
    int val = message.substring(3).toInt();
    legs[leg_idx -1].flight.origin.y = val;
}

void RYAction()
{
    int leg_idx = message.substring(2,3).toInt();
    int val = message.substring(3).toInt();
    legs[leg_idx -1].flight.radius.y = val;
 
}

void RXAction()
{
    int leg_idx = message.substring(2,3).toInt();
    int val = message.substring(3).toInt();
}

void SPAction()
{
   int val = message.substring(2).toInt();
   TICK_DELAY = val;
}

void MVAction()
{
  int val = message.substring(2).toInt();
  if(val==1) Play();
  if(val==0) Stop();
  if(val==2) StepForward();
  if(val==3) Stand(); //StepBackward();
}

void Stop()
{
    STOPPING = true;
}
void Play()
{
    MOVING = true;
}

void Stand()
{
  MOVING = false;
  for(int i=0;i < 4;i++)
  {            
    set_leg(i,STAND[i]);
    //if(LegNames[legs[i].index] == "FRONT RIGHT")
    //{
      Serial.println("");
      Serial.print("Leg ");
      Serial.print(LegNames[legs[i].index]);
      Serial.print(" hip:");
      Serial.print(legs[i].hipknee.hip);
      Serial.print(" knee:");
      Serial.println(legs[i].hipknee.knee);
    //}
  }
}

void StepForward()
{
   for(int i=0;i < 4;i++)
  {
	legs[i].forward = true;
  } 
  MOVING = true;
  STOPPING = true;
}
void StepBackward()
{
  for(int i=0;i < 4;i++)
  {
	legs[i].forward = false;
  } 
  MOVING = true;
  STOPPING = true;
}


void walking()
{
    //this is the first tick on this step
    //find the current pose 
    if(CURRENT_TICK == 0)
    { 
        if(CHAT) Serial.println("Current tick:0 - find trajectory");
        for(int i=0;i < 4;i++)
        {            
            leg_trajectory_for_step(&legs[i],CURRENT_STEP);
        }
    }
  
    //position the legs based on the current tick and place on the current leg trajectory plan
    if(CHAT) Serial.print("Position leg to tick:");
    if(CHAT) Serial.println(CURRENT_TICK);
    
    for(int i=0;i < 4;i++)
    {
        leg_position_for_tick(&legs[i]);
    }
    
    //increase the tick
    CURRENT_TICK++;

    if(CURRENT_TICK >= GRANULARITY) //new step
    {
        STEP_COUNT++;
        CURRENT_STEP++;
        CURRENT_TICK = 0;
        
        if(CHAT) Serial.println("************* step completed *****************");
    }
    if(CURRENT_STEP >= STEPS_IN_WALK) //new cycle
    {
        WALK_CYCLE_COUNT++;
        CURRENT_STEP = 0;
        if(CHAT) Serial.println("*************** Cycle completed ****************");
               
        if(STOPPING)
        {         
          MOVING = false;
          STOPPING = false;
        }
    }
    delay(TICK_DELAY);
 
}

includes.h

Code: Select all


//Serial communication variables
String message = ""; 
boolean message_complete = false;

//Servo management variables and constants
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

// *************** SERVO IDS *************
#define HIP_RIGHT_BACK 0
#define KNEE_RIGHT_BACK 1

#define HIP_LEFT_BACK 2
#define KNEE_LEFT_BACK 3

#define HIP_RIGHT_FRONT 12
#define KNEE_RIGHT_FRONT 13

#define HIP_LEFT_FRONT 14
#define KNEE_LEFT_FRONT 15

// ************** DEFAULT GEOMETRY *********
#define FEMUR_SIZE 65
#define TIBIA_SIZE 45
#define POSE_WIDTH 10
#define POSE_HEIGHT 10
#define POSE_ORIGIN_Y 100
#define POSE_ORIGIN_X 0

// *************** COMMAND CONSTANTS ********
#define LEG_BL 0
#define LEG_FL 1
#define LEG_FR 2
#define LEG_BR 3
#define STOP 4

// ***************** DATA STRUCTURES ********
struct point {
    int x;
    int y;
};

struct arc {
  point origin;
  point radius;
  int start_angle;
  int end_angle;
};

struct line {
  point P1;
  point P2;
};

struct angles {
    int hip;
    int knee;
};

struct pose {
  int index;
  struct point width_height;
  struct point origin_offset;
};

struct leg {
  int index;
  struct angles position;
  struct point trajectory[10];
  struct arc flight;
  struct angles hipknee;
  boolean forward;
};

char* LegNames[]={"BACK LEFT", "FRONT LEFT", "FRONT RIGHT","BACK RIGHT"};

// *************** ARRAY OF SERVO IDS - CLOCKWISE DIRECTION STARTING WITH BACK LEFT
int servos[4][2] =
{
  {HIP_LEFT_BACK,KNEE_LEFT_BACK},
  {HIP_LEFT_FRONT,KNEE_LEFT_FRONT},
  {HIP_RIGHT_FRONT,KNEE_RIGHT_FRONT},
  {HIP_RIGHT_BACK,KNEE_RIGHT_BACK}
};

// *************** LEGS IN CLOCKWISE DIRECTION *******
struct leg legs[4] = {};


// *************** DEFAULT STAND POSE ********
int STAND[4] = {2,2,2,2};


// **************** GAIT DEFINITIONS ********
//this array has tree dimensions, the first level is the walk sequence
//this can be lateral, parallel, left turn or right turn
//the next dimension holds the five steps of each sequence
//the last dimension hold the four positions for each leg at the given step
//the order is as allways clockwise starting with the back left leg
int WALKS[4][4][4] =
{
  {
    { 2, 3, 1, 4 },  //4 //4 baglens
    { 3, 4, 2, 1 },  //1 //3
    { 4, 1, 3, 2 },  //2 //2
    { 1, 2, 4, 3 }   //3 //1
  },
  {
    { 2, 3, 1, 4 },  //4 //4 baglens
    { 3, 4, 2, 1 },  //1 //3
    { 4, 1, 3, 2 },  //2 //2
    { 1, 2, 4, 3 }   //3 //1
  },
/*
  {
     { 2, 1, 3, 4 },
     { 3, 2, 4, 0 },
     { 4, 3, 0, 1 },
     { 0, 4, 1, 2 }
     //{ 1, 0, 2, 3 }
  },*/
  {
     { 2, 3, 4, 1 },
     { 1, 2, 0, 2 },
     { 0, 1, 1, 3 },
     { 4, 0, 2, 4 }
     //{ 3, 4, 3, 0 }
  },
  {
     { 2, 3, 4, 1 },
     { 1, 2, 0, 2 },
     { 0, 1, 1, 3 },
     { 4, 0, 2, 4 }
     //{ 3, 4, 3, 0 }
  }
};

#define LATERAL_WALK 0
#define PARALLEL_WALK 1
#define LEFT_TURN 2
#define RIGHT_TURN 3




leg.ino

Code: Select all

/*****************************
Create a default Leg Structure
******************************/

struct leg make_leg(int idx)
{
  struct arc a;
  struct point o = { 0,0 };
  //struct point o = { POSE_ORIGIN_X, POSE_ORIGIN_Y };
  a.origin = o;
  
  struct point r = {POSE_WIDTH, POSE_HEIGHT};
  a.radius = r;
  
  a.start_angle = 0;
  a.end_angle = 180;
  
  struct leg lg;
  lg.flight = a;
  lg.index = idx;
  lg.forward = true;

  return lg;
}

void leg_position_for_tick(struct leg *L)
{
   if(CHAT)
   {
     Serial.print("tick position leg:");
     Serial.print(L->index);
     Serial.print(" x:");
     Serial.print(L->trajectory[CURRENT_TICK].x);
     Serial.print(" y:");
     Serial.println(L->trajectory[CURRENT_TICK].y);
   }
   
   int tk = CURRENT_TICK;
   
   L->hipknee = IK(FEMUR_SIZE,TIBIA_SIZE,L->trajectory[tk]);
   
   //mirow the angles for right side
   if(L->index > 1)
   {
      L->hipknee.hip = 180 - L->hipknee.hip;
      L->hipknee.knee = 180 - L->hipknee.knee;
   }
   
   if(CHAT)
   {
      Serial.print("leg:");
      Serial.print(L->index);
      Serial.print(" hip:");
      Serial.print(L->hipknee.hip);
      Serial.print(" knee:");
      Serial.println(L->hipknee.knee);
    }
    pwm.setPWM(servos[L->index][0],0,map(L->hipknee.hip, 0, 180, SERVOMIN, SERVOMAX));
    pwm.setPWM(servos[L->index][1],0,map(L->hipknee.knee, 0, 180, SERVOMIN, SERVOMAX));  
}

void set_leg(int idx, int pose_idx)
{
    //legs[idx]
    struct pose P;
    struct point p;
    struct point w_h = { POSE_WIDTH, POSE_HEIGHT };
    struct point off = { POSE_ORIGIN_X, POSE_ORIGIN_Y };
    P.index = pose_idx;
    P.width_height = w_h;
    P.origin_offset = off;
    p = point_for_pose(&P);
    
    legs[idx].hipknee = IK(FEMUR_SIZE,TIBIA_SIZE,p);  
    
    boolean mirrow = false;
    //check if right - mirrow the angles
    if(idx > 1)
    {
      mirrow = true;
      legs[idx].hipknee.hip = 180 - legs[idx].hipknee.hip;
      legs[idx].hipknee.knee = 180 - legs[idx].hipknee.knee;
    }
    
    if(CHAT)
    {
      Serial.print("leg:");
      Serial.print(idx);
      Serial.print(" mirrow:");
      Serial.print(mirrow);
      Serial.print(" step:");
      Serial.print(pose_idx);
      Serial.print(" hip:");
      Serial.print(legs[idx].hipknee.hip);
      Serial.print(" knee:");
      Serial.println(legs[idx].hipknee.knee);
    }
    
    pwm.setPWM(servos[idx][0],0,map(legs[idx].hipknee.hip, 0, 180, SERVOMIN, SERVOMAX));
    pwm.setPWM(servos[idx][1],0,map(legs[idx].hipknee.knee, 0, 180, SERVOMIN, SERVOMAX));
}

/**********************************************************
 * Inverse Kinematic function for a two link planar system.
 * Given the size of the two links an a desired position, 
 * it returns the angles for both links
 **********************************************************/
struct angles IK(int L1, int L2, struct point p)
{
    struct angles a;

    int L1P2 = L1 * L1;
    int L2P2 = L2 * L2;
    int x2 = p.x * p.x;
    int y2 = p.y * p.y;

    //calculates the distance beetween the first link and the endpoint
    float d = sqrt( x2 + y2 );
    float c = min( d, L1 + L2 );
    float c2 = c * c;

    //Find wich cartesian quadrant the solution should be in
    float D = atan2(p.y,p.x);

    //calculates the angle between the distance segment and the first link
    float B = acos( (L1P2 - L2P2 + c2) / ( -2 * L1 * c) );

    //Add the quadrant information and convert from radias to angles 
    a.hip = (int)((D + B) * RAD_TO_DEG );
    
    if(a.hip > 180) a.hip -= 180;

    //calculate the angle between the first and second link
    float C = acos( (c2 - L1P2 - L2P2) / (-2 * L1 * L2) );
    
    //the knee angle is relative to the first link;
    a.knee = (int)( C * RAD_TO_DEG);
    
    //relate the angle to the x axis (horizon) and flip it to match the servo position
    a.knee = 180 - ( a.knee - a.hip );

    return a;
}



test.ino

Code: Select all

void test_point_for_pose()
{
  struct pose P;
  struct point p;
  struct point w_h = { POSE_WIDTH, POSE_HEIGHT };
  struct point off = { POSE_ORIGIN_X, POSE_ORIGIN_Y };
  P.width_height = w_h;
  P.origin_offset = off;
  
  for(int i=0;i < 5;i++)
  {
    P.index = i;    
    p = point_for_pose(&P);
    
    struct angles a = IK(FEMUR_SIZE,TIBIA_SIZE,p);  
    
    Serial.print("index:");
    Serial.print(i);
    Serial.print(" x:");
    Serial.print(p.x);
    Serial.print(" y:");
    Serial.print(p.y);
    Serial.print(" hip:");
    Serial.print(a.hip);
    Serial.print(" knee:");
    Serial.println(a.knee);
  }
}

void test_ik()
{
    struct point p1;
    p1.x = 0; 
    p1.y = -100;
  
    struct angles a = IK(80,65,p1);  
    
    p1.x = 0;
    p1.y = -70;
    a = IK(80,65,p1);  
  
    p1.x = 40;
    p1.y = -100;
    a = IK(80,65,p1);  
  
    p1.x = -40;
    p1.y = -100;
    a = IK(80,65,p1); 
}

void test_linear_trajectory()
{
   struct point S[10] = {};
   struct point P1 = {0,0};
   struct point P2 = {100,100};
   struct line l = { P1,P2 };
  
   int granularity = 10;
   linear_trajectory(S, granularity, &l, true);
   for(int i=0;i < granularity;i++)
   {
      Serial.print("step:");
      Serial.print(i);
      Serial.print(" x:");
      Serial.print(S[i].x);
      Serial.print(" y:");
      Serial.println(S[i].y);
   }
}

void test_elliptical_trajectory()
{
    struct point S[10] = {};
    struct point P1 = {0,0};
    struct point P2 = {100,100};
    struct line l = { P1,P2 };
    
    int granularity = 10;
    struct arc a;
    a.origin.x = 0;
    a.origin.y = 0; 
    a.radius.x = 40;
    a.radius.y = 40; 
    a.start_angle = 0;
    a.end_angle = 180;
    
    elliptical_trajectory(S,granularity, &a, false);
   
    for(int i=0;i < granularity;i++)
    {
      Serial.print("step:");
      Serial.print(i);
      Serial.print(" x:");
      Serial.print(S[i].x);
      Serial.print(" y:");
      Serial.println(S[i].y);
    }
}

void test_reverse_trajectory()
{
  
    struct point S[10] = {};
    struct point P1 = {0,0};
    struct point P2 = {100,100};
    struct line l = { P1,P2 };
    
    int granularity = 4;
    struct arc a;
    a.origin.x = 0;
    a.origin.y = 0; 
    a.radius.x = 40;
    a.radius.y = 40; 
    a.start_angle = 0;
    a.end_angle = 180;
    
    elliptical_trajectory(S,granularity, &a, false);
   
    Serial.println("********************");
    for(int i=0;i < granularity;i++)
    {
      Serial.print("step:");
      Serial.print(i);
      Serial.print(" x:");
      Serial.print(S[i].x);
      Serial.print(" y:");
      Serial.println(S[i].y);
    }
    
    reverse_trajectory(S);
    
    Serial.println("********************");
    for(int i=0;i < granularity;i++)
    {
      Serial.print("step:");
      Serial.print(i);
      Serial.print(" x:");
      Serial.print(S[i].x);
      Serial.print(" y:");
      Serial.println(S[i].y);
    }
}

trajectory.ino

Code: Select all

/***************************************************
 * Find the trajectory points between two steps,
 * The trajectory can be linear (ground phase) 
 * or elliptical (flight phase)
 ***************************************************/
 
void leg_trajectory_for_step(struct leg *L, int s)
{
      //find the current step 
      int c_step = WALKS[CURRENT_WALK][s][L->index];
      
      //find the next step
      int n_step = WALKS[CURRENT_WALK][(s == (STEPS_IN_WALK - 1))? 0:s + 1][L->index];
      
     
      if(CHAT)
      {
        Serial.print("LEG:");
        Serial.print(LegNames[L->index]);
        Serial.print("current step:");
        Serial.print(c_step);
        Serial.print(" next step:");
        Serial.println(n_step);
      }
      
      boolean flight = false;
      if(c_step == 4 && n_step == 1) //we are walking forwards
      {
         //create an elliptical trajectory
         elliptical_trajectory(L->trajectory, GRANULARITY, &L->flight,false);
         flight = true;
      }
      
      if(c_step == 1 && n_step == 4)  //we are walking backwards
      {
         elliptical_trajectory(L->trajectory, GRANULARITY, &L->flight,false);
         reverse_trajectory(L->trajectory);
         flight = true;
      }
      
      if(!flight)
      {
          struct pose P1;
          struct point p1;
          struct point p2;
          struct point w_h = { POSE_WIDTH, POSE_HEIGHT };
          struct point off = { POSE_ORIGIN_X, POSE_ORIGIN_Y };

          P1.width_height = w_h;
          P1.origin_offset = off;
          
          P1.index = c_step;    
          p1 = point_for_pose(&P1);
          P1.index = n_step;
          p2 = point_for_pose(&P1);
        
          struct line l = { p1,p2 };

          linear_trajectory(L->trajectory, GRANULARITY, &l, true);
      }	

}

/******************************************************************
 * Given a pose structure return a cartesian point
 * A pose can have an index from 0 to 4, where 0 is
 * the swing phase and 1 to four are the support phase
 * The pose contains the relevant geometry to generate the point
 ******************************************************************/
 
struct point point_for_pose(struct pose *P)
{
	struct point p;
        p.x = P->origin_offset.x;
        p.y = P->origin_offset.y;

	if(P->index == 0)
	{
		p.y = p.y - P->width_height.y;
	}
        if(P->index == 1)
	{
		p.x = p.x + P->width_height.x;
	}
        if(P->index == 2)
	{
		p.x = p.x + (P->width_height.x / 3);
	}
        if(P->index == 3)
	{
		p.x = p.x - (P->width_height.x / 3);
	}
        if(P->index == 4)
	{
		p.x = p.x - P->width_height.x;
	}

	return p;
}


/**************************************************************************
 * generate a linear trajectory plan (steps) between two points
 * with the required granularity (number of points)
 * The last parameter tells if you want the first point in the plan or not
 **************************************************************************/
 
void linear_trajectory(struct point steps[], int granularity, struct line *L, boolean skip_start_point)
{
       // find the slopes/delta
       float delta_x = L->P2.x - L->P1.x;
       float delta_y = L->P2.y - L->P1.y;
       
	//calculate the distance between the two points
        float distance = sqrt( ((delta_x) * (delta_x)) + ((delta_y) * (delta_y)) );
  
        //divide the line int the required number of points
        //decrease the granularity one step to be able to include the end point
        int skip = (skip_start_point)? 0:1;
        float step_size = distance / (granularity - skip);

        float c_step = (skip_start_point)? step_size:0;
        
        for(int i=0;i < granularity;i++)
        {
              float inc = c_step / distance;
              float x = L->P1.x + (inc * delta_x);
              float y = L->P1.y + (inc * delta_y);
                 
              steps[i].x = (int)x;
              steps[i].y = (int)y;
              c_step+= step_size;
        }
        
}

/**************************************************************************
 * Generate an elliptical trajectory plan 
 * with the required granularity for the provided arc struct. 
 * The last parameter tells if you want the first point in the plan or not
 **************************************************************************/
 
void elliptical_trajectory(struct point steps[], int granularity, struct arc *a, boolean skip_start_point)
{
      //divide the angles int the required number of points
      //decrease the granularity one step to be able to include the end point
      int skip = (skip_start_point)? 0:1;
      
      float step_size = (a->end_angle - a->start_angle) / (granularity - skip);
      float c_angle = a->start_angle;
      
      if(skip_start_point) c_angle+= step_size;
      
      for(int i=0;i < granularity;i++)
      {
        float x = a->origin.x + a->radius.x * cos(radians(c_angle));
        float y = a->origin.y + a->radius.y * sin(radians(c_angle));
       
        steps[(granularity - 1) - i].x = (int)x  + POSE_ORIGIN_X;
        steps[(granularity - 1) - i].y = POSE_ORIGIN_Y - (int)y ;
        c_angle+= step_size;
      }
}

void reverse_trajectory(struct point steps[])
{
    struct point t[10];
    for(int i=0;i < GRANULARITY;i++)
    {
      t[GRANULARITY - i -1] = steps[i];
    }
    for(int i=0;i < GRANULARITY;i++)
    {
      steps[i] = t[i];
    }
    
}


Let me know guys...!!. You will love this doing bionic robotics. Using Arduino Only feedback


Image
User avatar
dondon pramis
Inventor
Inventor
Contact:
Location: Philippines
Posts: 42
Joined: Sun Feb 12, 2017 8:28 am

Re: Bionic Robot Arduino

Sun Sep 16, 2018 2:12 am

It is now Updated Here:

>>> viewtopic.php?f=28&t=361 <<<

Return to “ARDUINO AND RASPBERRY PI”

Links

In total there are 12 users online :: 1 registered, 0 hidden and 11 guests
Registered users: Bing [Bot]
Most users ever online was 156 on Sun Jun 17, 2018 7:42 am
Total posts 439
Total topics 318
Total members 62
Our newest member anonymous-kali
No birthdays today