mangavalk
fe472bfce9
From Electronics\Hexapod\Arduino sketches 2011-2012\hexapod_oud (banner 'Version 3.0').
80 lines
1.5 KiB
C++
80 lines
1.5 KiB
C++
#include "SSC32.h"
|
|
|
|
void SSC32::setup()
|
|
{
|
|
Serial1.begin(38400);
|
|
Serial.begin(38400);
|
|
time = 0;
|
|
walktime = 0;
|
|
}
|
|
|
|
void SSC32::MoveHead (int angel_from_mid)
|
|
{
|
|
Serial1.print(" #");
|
|
Serial1.print(9);
|
|
Serial1.print(" P");
|
|
Serial1.print(ConverttoMS((90 + angel_from_mid), smin[0], smax[0]));
|
|
Serial1.print(" T");
|
|
Serial1.println(100);
|
|
}
|
|
|
|
void SSC32::Render()
|
|
{
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
RAW_2_SERVO[0+(i*3)] = ConverttoMS(DEG_2_CON[0+(i*3)], smin[0], smax[0]);
|
|
RAW_2_SERVO[1+(i*3)] = ConverttoMS(DEG_2_CON[1+(i*3)], smin[1], smax[1]);
|
|
RAW_2_SERVO[2+(i*3)] = ConverttoMS(DEG_2_CON[2+(i*3)], smin[2], smax[2]);
|
|
|
|
if (i != 0)
|
|
Serial.print(", ");
|
|
else
|
|
Serial.print("Ang: ");
|
|
|
|
Serial.print(DEG_2_CON[0+(i*3)]);
|
|
Serial.print(", ");
|
|
Serial.print(DEG_2_CON[1+(i*3)]);
|
|
Serial.print(", ");
|
|
Serial.print(DEG_2_CON[2+(i*3)]);
|
|
}
|
|
Serial.println(";");
|
|
}
|
|
|
|
int SSC32::ConverttoMS(int i, int low, int high)
|
|
{
|
|
int tmp = ( i * ( ( high - low ) / 180 ) );
|
|
return low + tmp;
|
|
}
|
|
|
|
void SSC32::toServos ()
|
|
{
|
|
// puss to servos
|
|
|
|
// var. SSC32.
|
|
// 0 | 3 // 16 | 0
|
|
// 1 | 4 // | | |
|
|
// 2 | 5 // 24 | 8
|
|
|
|
for (int i=0; i<9; i++)
|
|
{
|
|
Serial1.print(" #");
|
|
Serial1.print(16+i);// 18 22
|
|
Serial1.print(" P");
|
|
Serial1.print(RAW_2_SERVO[9+i]);
|
|
}
|
|
|
|
for (int i=0; i<9; i++)
|
|
{
|
|
Serial1.print(" #");
|
|
Serial1.print(i);
|
|
Serial1.print(" P");
|
|
Serial1.print(RAW_2_SERVO[i]);
|
|
}
|
|
|
|
Serial1.print(" T");
|
|
Serial1.println(walktime);
|
|
}
|
|
|
|
|
|
|