﻿ Animal Robots » Blog Archive » Vector model (2)
Jan 25

The first steps have been made to calculate a vector based model of the snake based on the servo positions and the servo length as definied previously. The code is indeed simple, based on:

ServoPositions   := 570;
ServoPositions   := 692;
ServoPositions   := 440;
ServoPositions   := 372;
ServoPositions   := 405;
ServoPositions   := 665;
ServoPositions   := 598;
ServoPositions   := 664;
ServoPositions   := 350;
ServoPositions   := 349;
ServoPositions  := 582;

below snake is drawn: using below procedure to calculate the joint positions:

procedure TMainWindow.CalculateJoints;
var Servo  : Integer;
dAngle : Real;
dX     : Real;
dY     : Real;
begin
JointLocations .X := 0;  // Tip of the head is reference point
JointLocations .Y := 0;
JointLocations .X := (0-cv_ServoLength);
JointLocations .Y := 0;

dAngle := 180;

for Servo := 0 to (cv_MaxServo-1) do
begin
dAngle := (dAngle + ((((1023-ServoPositions [Servo])/3.41)+30)-180));
dX     := (Cos((dAngle/360)*(2*Pi))*cv_ServoLength);
dY     := (Sin((dAngle/360)*(2*Pi))*cv_ServoLength);

JointLocations [Servo+2].X := (JointLocations [Servo+1].X+dX);
JointLocations [Servo+2].Y := (JointLocations [Servo+1].Y+dY);
end;
end;

In order to map the splines over the joints two additional points are needed. One point one servo length behind the tip of the tail and one point one servo length in front of the tip of the head. The point behind the tail is not known when the snake is powered on, by default the mirrored point can be used. As a result a curved spline is drawn, when the snake moved one servo length forward that the previous tail position can be used. The point in front of the snake is also not known but this should be on the path to follow. As a result a “path to follow” generator needs to be developed, although at this point I have no idea how this should look like it is clear this will be an important part of the snake. It should map a basic sine but change the sine if obstacles are in the way or if a corner must be taken.