Centre de ressources
Heol Robotics


Affichage du code : mouvement.ino

					/* ********************************************************************/
/*  For       : © Heol Robotics - http://heol.io
/*  File      : movements.ino
/*  Others    : Based on Arduino library by hackerspace-adelaide    
/* ********************************************************************/

#define centralPosition     512
#define defaultSpeed        500
#define defaultTorque       1023

/*===============================================================================
  FUNCTION:     initMotorsSettings
  DESCRIPTION:  This function set settings of all motors
  PARAMETERS:   none
  RETURNS:      none   
===============================================================================*/
void initMotorsSettings()
{
  Heol.setJointSpeed(all_motors, defaultSpeed);
  Heol.TorqueON(all_motors);
}

/*===============================================================================
  FUNCTION:     getMotorsData
  DESCRIPTION:  This function get some informations about specified motor
  PARAMETERS:   Id motor concerned
  RETURNS:      none   
===============================================================================*/
void getMotorsData(int heolMotorId)
{
  int jointPosition, jointSpeed;
  char charJointPosition[] = "", charJointSpeed[] = "";
  
  /* Read the data from the motors */
  jointPosition = Heol.getJointPosition(heolMotorId);
  jointSpeed = Heol.getJointSpeed(heolMotorId);

  /* Convert the int > char */
  sprintf(charJointPosition, "Pos: %d", jointPosition);
  sprintf(charJointSpeed, "Vit: %d", jointSpeed);
}

/*===============================================================================
  FUNCTION:     getMotorsData
  DESCRIPTION:  This function makes the led of all motors flash
  PARAMETERS:   none
  RETURNS:      none   
===============================================================================*/
void blinkLeds()
{
  int i = 0;

  for(i = 0; i < 7; i++)
  {
    Heol.LED(all_motors, &ledsColors[i]); 
    delay(100);
  }
}

/*===============================================================================
  FUNCTION:     standUp
  DESCRIPTION:  This function allows the robot to put itself up
  PARAMETERS:   none
  RETURNS:   	  none   
===============================================================================*/
void standUp()
{
	Heol.moveJoint(l_hip_z , centralPosition);
	Heol.moveJoint(l_upper_arm_z , centralPosition);
	Heol.moveJoint(l_shoulder_y, centralPosition);
	Heol.moveJoint(l_thigh_x , centralPosition);
	Heol.moveJoint(l_hip_y , centralPosition);
	Heol.moveJoint(l_foot_y, centralPosition);
	Heol.moveJoint(l_ankle_x, centralPosition);
	Heol.moveJoint(l_forearm_y, centralPosition);
	Heol.moveJoint(l_knee_x , centralPosition);
	Heol.moveJoint(l_shoulder_x, centralPosition);
	
	Heol.moveJoint(r_hip_z , centralPosition);
	Heol.moveJoint(r_upper_arm_z , centralPosition);
	Heol.moveJoint(r_shoulder_y, centralPosition);
	Heol.moveJoint(r_thigh_x, centralPosition);
	Heol.moveJoint(r_hip_y, centralPosition);
	Heol.moveJoint(r_foot_y, centralPosition);
	Heol.moveJoint(r_knee_x, centralPosition);
	Heol.moveJoint(r_forearm_y, centralPosition);
	Heol.moveJoint(r_knee_x, centralPosition);
	Heol.moveJoint(r_shoulder_x, centralPosition);
	
	Heol.moveJoint(head_z , centralPosition);
	Heol.moveJoint(spine_z, centralPosition);
	Heol.moveJoint(chest_x, centralPosition);
}

/*===============================================================================
  FUNCTION:     walkForward
  DESCRIPTION:  The robot moves straight
===============================================================================*/
void walkForward()
{
	Heol.moveJoint(head_z, 1023);
}

/*===============================================================================
  FUNCTION:     turnRight
  DESCRIPTION:  The robot turns right
===============================================================================*/
void turnRight()
{
	Heol.moveJoint(head_z, 1023);
}

/*===============================================================================
  FUNCTION:     turnLeft
  DESCRIPTION:  The robot turns left
===============================================================================*/
void turnLeft()
{
	Heol.moveJoint(head_z, 1023);
}

/*===============================================================================
  FUNCTION:     walkForward
  DESCRIPTION:  The robot moves his head to say NO
  PARAMETERS:   Number of head rotations
===============================================================================*/
void sayNo(int numberOfRotation)
{
	int i = 0;
	
	for(i = 0; i < numberOfRotation; i++)
	{
		Heol.moveJoint(head_z, 512);
    delay(300);
		Heol.moveJoint(head_z, 0);
    delay(300);
		Heol.moveJoint(head_z, 1023);
		delay(300);
    Heol.moveJoint(head_z, 512);
	}
}

/*===============================================================================
  FUNCTION:     armsUpright
  DESCRIPTION:  The robot put his arms upright 
===============================================================================*/
void armsUpright()
{
  Heol.moveJoint(l_shoulder_y, 0);
  Heol.moveJoint(l_upper_arm_z, 0);
  Heol.moveJoint(l_forearm_y, 0);

  Heol.moveJoint(r_shoulder_y, 0);
  Heol.moveJoint(r_upper_arm_z, 0);
  Heol.moveJoint(r_forearm_y, 0);
}

/*===============================================================================
  FUNCTION:     seated
  DESCRIPTION:  The robot sit
===============================================================================*/
void seated()
{
	Heol.moveJoint(l_hip_z , 502);
	Heol.moveJoint(l_upper_arm_z , 470);
	//Heol.moveJoint(l_shoulder_y, ??);
	Heol.moveJoint(l_thigh_x , 178);
	//Heol.moveJoint(l_hip_y , ??);
	Heol.moveJoint(l_foot_y, 533);
	Heol.moveJoint(l_ankle_x, 439);
	//Heol.moveJoint(l_forearm_y, ??);
	Heol.moveJoint(l_knee_x , 420);
	Heol.moveJoint(l_shoulder_x, 460);
			  
	Heol.moveJoint(r_hip_z , 564);
	Heol.moveJoint(r_upper_arm_z , 450);
	Heol.moveJoint(r_shoulder_y, 460);
	Heol.moveJoint(r_thigh_x, 157);
	Heol.moveJoint(r_hip_y, 485);
	Heol.moveJoint(r_foot_y, 483);
	Heol.moveJoint(r_knee_x, 483);
	Heol.moveJoint(r_forearm_y, 77);
	//Heol.moveJoint(r_knee_x, ??);
	Heol.moveJoint(r_shoulder_x, 475);
			  
	Heol.moveJoint(head_z , centralPosition);
	Heol.moveJoint(spine_z, centralPosition);
	Heol.moveJoint(chest_x, centralPosition);
}

/*===============================================================================
  FUNCTION:     getup
  DESCRIPTION:  The robot rises 
===============================================================================*/
void getup()
{
	Heol.moveJoint(head_z, 1023);
}
				

Centre de ressources © Heol Robotics 2017 - Tous droits réservés