Hi Their, I hope someone can help me. I am not new to developing but I am new to Arduino's. I have my self a MEGA 2560, which I am using to build a eight legged robot's with.
I have Already had some success with this but I have not started wiring the code to allow me to control this robot with a USB wired tether. For the first 7 of my 8 legs this works fine. But my Last leg number 8 things are very strange indeed.
I have my Arduino echo the Byte array that it gets back to the Host Computer. And this array of Data is getting Changed the first 2 bytes in the byte array are altered. I expect to see a 76 , 8 But I get 180, 0. For all other legs it's working fine it's just this last leg.
Here is the code that I am using to drive this project.
This is my Arduino code.
#include <Servo.h>
#include <RobotLeg.h>
#include <MyRobot.h>
int MainPowerPin = 2;
MyRobot Bot;
byte Command[10];
boolean Cmd_rec = false;
int idx=0;
int Commandsize=0;
void setup()
{
pinMode(MainPowerPin,OUTPUT);
digitalWrite(MainPowerPin,LOW);
Serial.begin(57600);
Serial.println("Robot- Ready!");
Bot.AddLeg(39,41,43);
Bot.AddLeg(45,47,49);
Bot.AddLeg(51,50,48);
Bot.AddLeg(46,44,42);
Bot.AddLeg(53,52,33);
Bot.AddLeg(36,40,38);
Bot.AddLeg(31,29,27);
Bot.AddLeg(34,32,26);
}
void loop()
{
GetCommand();
if(Cmd_rec)
{
ProcessCommand();
Serial.write(Command,Commandsize);
Serial.println("ACK");
}
}
boolean GetCommand()
{
Cmd_rec = false;
while (Serial.available() > 0)
{
if(idx < 39)
{
Command[idx] = Serial.read();
idx++;
if(Command[idx-1] == 59) //59 is ';'
{
Command[idx] = '\0';
Commandsize = idx;
idx = 0;
Cmd_rec = true;
return true;
}
}
else
{
Serial.println("Debug - Input Error - Input Too Long or No Terminitor Sent!");
Serial.println("ACK");
idx = 0;
return false;
}
}
return false;
}
void ProcessCommand()
{
switch(Command[0])
{
case 76:
//Processing LEg Movement
Bot.MoveLegPart(Command[1],Command[2],Command[3],Command[4]);
break;
case 80:
//Switch Power on to Servo's
switch (Command[1])
{
case 43:
//ON
digitalWrite(MainPowerPin,HIGH);
break;
case 44:
//OFF
digitalWrite(MainPowerPin,LOW);
break;
}
break;
}
}
Here is my MyRobot.h File
#ifndef MyRobot_h
#define MyRobot_h
#include <Arduino.h>
#include <../RobotLeg/RobotLeg.h>
enum LegPart
{
lp_Hip_V, lp_Hip_H, lp_Knee
};
class MyRobot
{
public:
MyRobot();
void AddLeg(RobotLeg Leg);
void AddLeg(int Hip_Hor_Pin, int Hip_Ver_Pin, int Knee_Pin);
void MoveLegPart(int Legnum,LegPart part, Direction dir, int ammount);
void MoveLegPart(byte Legnum,byte part, byte dir, byte ammount);
private:
RobotLeg _legs[7];
int _leg_count;
};
#endif
Here is my MyRobot.cpp file
#include <MyRobot.h>
MyRobot::MyRobot()
{
_leg_count = 0;
}
void MyRobot::AddLeg(RobotLeg Leg)
{
_legs[_leg_count] = Leg;
_leg_count++;
}
void MyRobot::AddLeg(int Hip_Hor_Pin, int Hip_Ver_Pin, int Knee_Pin)
{
RobotLeg L;
L.SetPins(Hip_Hor_Pin,Hip_Ver_Pin,Knee_Pin);
_legs[_leg_count] = L;
_leg_count++;
}
void MyRobot::MoveLegPart(int Legnum,LegPart part, Direction dir, int ammount)
{
int res = Legnum % 2;
switch(dir)
{
case UP:
if(res > 0)
{
//left
ammount = ammount -(ammount * 2);
}
else
{
//right
}
break;
case Down:
if(res > 0)
{
//left
}
else
{
//right
ammount = ammount -(ammount * 2);
}
break;
}
switch(part)
{
case lp_Hip_V:
_legs[Legnum - 1].SetHip_V(ammount,Offset);
break;
case lp_Hip_H:
_legs[Legnum - 1].SetHip_H(ammount,Offset);
break;
case lp_Knee:
_legs[Legnum - 1].SetKnee(ammount,Offset);
break;
}
}
void MyRobot::MoveLegPart(byte Legnum,byte part, byte dir, byte ammount)
{
Direction D;
LegPart P;
int leg = int(Legnum);
int amt = int(ammount);
switch (dir)
{
case 43:
D = UP;
break;
case 45:
D = Down;
break;
}
switch(part)
{
case 65:
P = lp_Hip_V;
break;
case 66:
P = lp_Hip_H;
break;
case 67:
P = lp_Knee;
break;
}
MoveLegPart(leg,P,D,amt);
}
Here is my RobotLeg.h file
#ifndef RobotLeg_h
#define RobotLeg_h
#include <../Servo/Servo.h>
enum Direction
{
UP, Down
};
enum MoveType
{
Extact, Offset
};
class RobotLeg
{
public:
RobotLeg();
void SetPins(int Hip_Hor_Pin, int Hip_Ver_Pin, int Knee_Pin);
void SetHip_H(int Angle,MoveType Ty);
void SetHip_V(int Angle,MoveType Ty);
void SetKnee(int Angle,MoveType Ty);
private:
int _Hip_Hor_Angle;
int _Hip_Ver_Angle;
int _Knee_Angle;
int _Delay;
int _Hip_Hor_Offset;
int _Hip_Ver_Offset;
int _Hip_Hor_Pin;
int _Hip_Ver_Pin;
int _Knee_Pin;
Servo _Hip_Hor;
Servo _Hip_Ver;
Servo _Knee;
int AngleCalculator(int cur, int changeammount);
};
#endif
here is my RobotLeg.ccp file
#include <RobotLeg.h>
#include <../Servo/Servo.h>
#include <Arduino.h>
RobotLeg::RobotLeg()
{
int test = 1;
_Delay = 0;
}
void RobotLeg::SetPins(int Hip_Hor_Pin, int Hip_Ver_Pin, int Knee_Pin)
{
_Hip_Hor_Pin = Hip_Hor_Pin;
_Hip_Ver_Pin = Hip_Ver_Pin;
_Knee_Pin = Knee_Pin;
_Hip_Hor.attach(_Hip_Hor_Pin);
_Hip_Ver.attach(_Hip_Ver_Pin);
_Knee.attach(Knee_Pin);
SetHip_H(90,Extact);
SetHip_V(90,Extact);
SetKnee(90,Extact);
}
void RobotLeg::SetHip_H(int Angle,MoveType Ty)
{
switch(Ty)
{
case Extact:
_Hip_Hor_Angle = Angle;
break;
case Offset:
_Hip_Hor_Angle = AngleCalculator( _Hip_Hor_Angle, Angle);
break;
}
_Hip_Hor.write(_Hip_Hor_Angle);
}
void RobotLeg::SetHip_V(int Angle,MoveType Ty)
{
switch(Ty)
{
case Extact:
_Hip_Ver_Angle = Angle;
break;
case Offset:
_Hip_Ver_Angle = AngleCalculator(_Hip_Ver_Angle , Angle);
break;
}
_Hip_Ver.write(_Hip_Ver_Angle);
}
void RobotLeg::SetKnee(int Angle,MoveType Ty)
{
switch(Ty)
{
case Extact:
_Knee_Angle = Angle;
break;
case Offset:
_Knee_Angle = AngleCalculator(_Knee_Angle , Angle);
break;
}
_Knee.write(_Knee_Angle);
}
int RobotLeg::AngleCalculator(int cur, int changeammount)
{
int res = cur + changeammount;
if(res < 0)
{
return 0;
}
if(res > 180)
{
return 180;
}
return res;
}
I hope that someone can shed some light on why this is happening.
Sorry for so much code but I do like to work with proper objects.
Thanking you all in advance.
