// measure distances over a 180 degree arc and save them into the array arcPingDistances with size ARCPING_STEPS
void Servotor32::arcPing() {
// min and max pos for servo
int minPos = 500;
int maxPos = 2500;
for(int i=0;i<ARCPING_STEPS; i++)
{
// calculate servopos for current step.
int pos = (int)((float)(maxPos - minPos) / (ARCPING_STEPS-1) * i + minPos);
// set servo to desired pos
changeServo(31,pos);
// if its the first position, give the servo enough time to go there from any previous position
if (i == 0)
{
delay_ms(300);
}
else // if its not the first step, then the position-difference and therefore time needed are smaller.
{
delay_ms(500 / ARCPING_STEPS);
}
// make a multiPing and save result into the array arcPingDistances
arcPingDistances[i] = multiPing(5);
}
// set servo back to middle pos, making head look forward
changeServo(31,1500);
// give the servo some time to reach pos
delay_ms(400);
// shut down servo
changeServo(31,-1);
}
void Servotor32::center()////////////////////////////////////////////////////////////
{
for(int i=0;i<32;i++)//CenterServos
{
changeServo(i,1500);
delay_ms(200);
changeServo(i,-1);
}
}
void Servotor32::killServos()///////////////////////////////////////////////////////////////////////////////
{
for(int i=0;i<32;i++)//KillServos
{
changeServo(i,-1);
}
}
void Servotor32::kickLeft()////////////////////////////////////////////////////////////////////////////////////
{
changeServo(10,700);
changeServo(9,700);
delay_ms(500);
changeServo(10,1500);
changeServo(9,1500);
delay_ms(1000);
changeServo(10,1950);
changeServo(9,1050);
delay_ms(400);
changeServo(10,-1);
changeServo(9,-1);
delay_ms(100);
}
// measure distances over a 180 degree arc and save them into the array arcPingDistances with size ARCPING_STEPS
void Servotor32::arcPing() {
for(int i=0;i<ARCPING_STEPS; i++)
{
int minPos = 500;
int maxPos = 2500;
int pos = (int)((float)(maxPos - minPos) / (ARCPING_STEPS-1) * i + minPos);
changeServo(31,pos);
if (i == 0)
{
delay_ms(300);
}
else
{
delay_ms(500 / ARCPING_STEPS);
}
arcPingDistances[i] = multiPing(5);
}
changeServo(31,1500);
delay_ms(400);
changeServo(31,-1);
}
void Servotor32::process(Stream *serial){
if(serial->available()) { //process input from the USB
char inChar = (char)serial->read();
switch(inChar){
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus(serial);
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
serial->println("SERVOTOR32_v2.0_lams");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
serial->println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
serial->println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'S': // make an arcPing and send distances back over serial
arcPing();
for(int i = 0; i < ARCPING_STEPS; i++)
{
serial->println(arcPingDistances[i]);
}
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
void Servotor32::kickFront()///////////////////////////////////////////////////////////////////////////////////////////
{
changeServo(25,1950);
changeServo(6,1950);
changeServo(24,1950);
changeServo(25,700);
changeServo(26,700);
delay_ms(500);
changeServo(25,1500);
changeServo(26,1500);
delay_ms(1000);
changeServo(24,1500);
changeServo(25,1950);
changeServo(26,1050);
delay_ms(400);
changeServo(6,-1);
changeServo(17,-1);
changeServo(25,-1);
changeServo(26,-1);
delay_ms(100);
}
void Servotor32::kickRight()////////////////////////////////////////////////////////////////////////////////////
{
changeServo(25,1950);
changeServo(17,1950);
changeServo(21,700);
changeServo(22,700);
delay_ms(500);
changeServo(21,1500);
changeServo(22,1500);
delay_ms(1000);
changeServo(21,1950);
changeServo(22,1050);
delay_ms(400);
changeServo(25,-1);
changeServo(17,-1);
changeServo(21,-1);
changeServo(22,-1);
delay_ms(100);
}
void Servotor32::rotateRight(){
// Picks up the knees from tripod 1, rotates them to the front most position, then places them down. Picks up tripod 2, then rotates tripod 1 (while still on the ground) to middle position, then replaces tripod 2
//tripod 1 == RF, LM, RB
//tripod 2 == LF, RM, LB
//put the servos in the correct position from stand up function
//Final positions: hips 1500, knees ~1950, ankles 1050
reset();
//pick up tripod 1
changeServo(25, 2000);
delay_ms(25);
changeServo(10, 2000);
delay_ms(25);
changeServo(17, 2000);
delay_ms(25);
//move tripod to forward most position
changeServo(24, 2200);
delay_ms(25);
changeServo(11, 2200);
delay_ms(25);
changeServo(16, 2200);
delay_ms(25);
//move tripod 1 down
changeServo(25, 1950);
delay_ms(25);
changeServo(10, 1950);
delay_ms(25);
changeServo(17, 1950);
delay_ms(25);
//move tripod 2 up
changeServo(6, 2200);
delay_ms(25);
changeServo(21, 2200);
delay_ms(25);
changeServo(14, 2200);
delay_ms(25);
//rotate tripod 1 back to original position
changeServo(24, 1500);
delay_ms(25);
changeServo(11, 1500);
delay_ms(25);
changeServo(16, 1500);
delay_ms(25);
// move tripod 2 back down
changeServo(6, 1950);
delay_ms(25);
changeServo(21, 1950);
delay_ms(25);
changeServo(14, 1950);
delay_ms(25);
reset();
}
void Servotor32::backward()////////////////////////////////////////////////////////////////////////////////////
{
//Two tripod system, denoted 1<LB,LF,RM> and 2<RB,RF,LM>
changeServo(25,-1);//Kill Knees 2
changeServo(17,-1);
changeServo(10,-1);
delay_ms(20);
changeServo(14,1950);//Stabalizer Feet 1
changeServo(6,1950);
changeServo(21,1950);
delay_ms(200);
changeServo(25,1750);//Pick Up Knees 2
changeServo(17,1750);
changeServo(10,1750);
delay_ms(200);
changeServo(25,-1);//Kill Knees 2
changeServo(17,-1);
changeServo(10,-1);
delay_ms(20);
changeServo(11,1750);//Reposition Hips 2
changeServo(24,1250);
changeServo(16,1250);
delay_ms(200);
changeServo(11,-1);//Kill Hips 2
changeServo(24,-1);
changeServo(16,-1);
delay_ms(200);
changeServo(9,950);//Reposition ankles 2
changeServo(18,950);
changeServo(26,950);
delay_ms(300);
changeServo(9,-1);//Kill ankles 2
changeServo(18,-1);
changeServo(26,-1);
delay_ms(20);
changeServo(25,1950);//Put down Knees 2, stay on
changeServo(17,1950);
changeServo(10,1950);
delay_ms(200);
changeServo(14,1750);//Pick up knees 1
changeServo(6,1750);
changeServo(21,1750);
delay_ms(100);
changeServo(14,-1);//Kill knees 1
changeServo(6,-1);
changeServo(21,-1);
delay_ms(10);
changeServo(22,950);//Reposition ankles 1
changeServo(13,950);
changeServo(5,950);
delay_ms(300);
changeServo(22,-1);//Kill ankles 1
changeServo(13,-1);
changeServo(5,-1);
delay_ms(20);
changeServo(7,1750);//Move hips
changeServo(20,1350);
changeServo(15,1750);
delay_ms(100);
changeServo(24,1750);
changeServo(11,1250);
changeServo(16,1750);
delay_ms(100);
changeServo(7,-1);
changeServo(11,-1);
changeServo(15,-1);
changeServo(24,-1);
changeServo(20,-1);
changeServo(16,-1);
delay_ms(10);
changeServo(14,1950);//Replant knees 1
changeServo(6,1950);
changeServo(21,1950);
delay_ms(100);
changeServo(25,1750);//Pick Up Knees 2
changeServo(17,1750);
changeServo(10,1750);
delay_ms(200);
changeServo(25,-1);//Kill Knees 2
changeServo(17,-1);
changeServo(10,-1);
delay_ms(20);
changeServo(7,1250);//Move hips
changeServo(20,1750);
changeServo(15,1250);
delay_ms(100);
changeServo(24,1250);
changeServo(11,1750);
changeServo(16,1250);
delay_ms(100);
changeServo(7,-1);
changeServo(11,-1);
changeServo(15,-1);
changeServo(24,-1);
changeServo(20,-1);
changeServo(16,-1);
delay_ms(10);
}
void Servotor32::reset()/////////////////////////////////////////////////////////////////////////////////
{
changeServo(6,1950); //Stabalizes knees
changeServo(10,1950);
changeServo(14,1950);
changeServo(25,1950);
changeServo(21,1950);
changeServo(17,1950);
delay_ms(100);
changeServo(17,1500);//RB
changeServo(16,1500);
delay_ms(200);
changeServo(18,500);
delay_ms(100);
changeServo(17,1950);
changeServo(18,1050);
delay_ms(200);
changeServo(18,-1);
changeServo(16,-1);
//changeServo(17,-1);
delay_ms(100);
changeServo(6,1500);//LF
changeServo(7,1500);
delay_ms(200);
changeServo(5,600);
delay_ms(100);
changeServo(6,1950);
changeServo(5,1050);
delay_ms(200);
changeServo(5,-1);
//changeServo(6,-1);
changeServo(7,-1);
delay_ms(100);
changeServo(21,1500);//RM
changeServo(20,1500);
delay_ms(200);
changeServo(22,500);
delay_ms(100);
changeServo(21,1950);
changeServo(22,1050);
delay_ms(200);
changeServo(22,-1);
changeServo(20,-1);
//changeServo(21,-1);
delay_ms(100);
changeServo(25,1500);//RF
changeServo(24,1500);
delay_ms(200);
changeServo(26,600);
delay_ms(100);
changeServo(25,1950);
changeServo(26,1050);
delay_ms(200);
changeServo(24,-1);
changeServo(26,-1);
//changeServo(25,-1);
delay_ms(100);
changeServo(14,1500);//LB
changeServo(15,1500);
delay_ms(200);
changeServo(13,500);
delay_ms(100);
changeServo(14,1950);
changeServo(13,1050);
delay_ms(200);
changeServo(13,-1);
changeServo(15,-1);
//changeServo(14,-1);
delay_ms(100);
changeServo(6,-1);
changeServo(10,-1);
changeServo(14,-1);
changeServo(25,-1);
changeServo(21,-1);
changeServo(17,-1);
delay_ms(10);
changeServo(10,1500);//LM
changeServo(11,1500);
delay_ms(200);
changeServo(9,500);
delay_ms(100);
changeServo(10,1950);
changeServo(9,1050);
delay_ms(200);
changeServo(9,-1);
changeServo(10,-1);
//changeServo(11,-1);
delay_ms(100);
}
void Servotor32::standUp()//////////////////////////////////////////////////////////////////////////
{
changeServo(7,1950);//Hip position
changeServo(11,1500);
changeServo(15,1250);
changeServo(24,1050);
changeServo(20,1500);
changeServo(16,1750);
delay_ms(200);
for(int i=0;i<32;i++)//KillServos
{
changeServo(i,-1);
}
changeServo(6,900);//Knee position
changeServo(10,900);
changeServo(14,900);
changeServo(25,900);
changeServo(21,900);
changeServo(17,900);
delay_ms(200);
for(int i=0;i<32;i++)//KillServos
{
changeServo(i,-1);
}
changeServo(5,600);//Ankle position
changeServo(9,600);
changeServo(13,600);
changeServo(26,600);
changeServo(22,600);
changeServo(18,600);
delay_ms(500);
for(int i=0;i<32;i++)//KillServos
{
changeServo(i,-1);
}
int knee[6] = {6,21,14,25,10,17};
int ankle[6] = {5,22,23,26,9,18};
//Stand
int ankAngle = 600;
for(int angle=1300; angle<2100; angle+=30)
{
for(int h=0;h<6;h++)
{
changeServo(knee[h],angle);
changeServo(ankle[h],ankAngle);
delay_ms(100);
changeServo(ankle[h],-1);
}
ankAngle+=15;
}
reset();
for(int i=0;i<32;i++)//KillServos
{
changeServo(i,-1);
}
}
void Servotor32::processChar(char inChar){
switch(inChar){
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus();
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
Serial.println("SERVOTOR32_v2.0");
Serial1.println("SERVOTOR32_v2.0");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
Serial.println("All Centered");
Serial1.println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
Serial.println("All Turned Off");
Serial1.println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'Q':
Serial.print("CM: ");
Serial.println(ping());
Serial1.print("CM: ");
Serial1.println(multiPing(10));
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
void Servotor32::processChar(char inChar){
if (binary) {
switch(inChar){
case '\r':
case '\n':
binary = false;
changeServo(5,inAllPos[0]);
changeServo(6,inAllPos[1]);
changeServo(7,inAllPos[2]);
changeServo(9,inAllPos[3]);
changeServo(10,inAllPos[4]);
changeServo(11,inAllPos[5]);
changeServo(13,inAllPos[6]);
changeServo(14,inAllPos[7]);
changeServo(15,inAllPos[8]);
changeServo(16,inAllPos[9]);
changeServo(17,inAllPos[10]);
changeServo(18,inAllPos[11]);
changeServo(20,inAllPos[12]);
changeServo(21,inAllPos[13]);
changeServo(22,inAllPos[14]);
changeServo(24,inAllPos[15]);
changeServo(25,inAllPos[16]);
changeServo(26,inAllPos[17]);
changeServo(31,inAllPos[18]);
break;
default:
inAllPos[numCount] = byte(inChar)*10;
numCount++;
break;
}
}
else {
switch(inChar){
case '$':
binary = true;
numCount = 0;
break;
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus();
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
Serial.println("SERVOTOR32_v2.1a");
Serial1.println("SERVOTOR32_v2.1a");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
Serial.println("All Centered");
Serial1.println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
Serial.println("All Turned Off");
Serial1.println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'S': // make an arcPing and send distances back over serial
arcPing();
for(int i = 0; i < ARCPING_STEPS; i++)
{
Serial.println(arcPingDistances[i]);
Serial1.println(arcPingDistances[i]);
}
delay_ms(200);
break;
case 'M': // send multiple distance readings back over serial
Serial.println(multiPing(5));
delay_ms(200);
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
void Servotor32::process(Stream *serial){
if(serial->available()) { //process input from the serial stream (USB or Serial)
char inChar = (char)serial->read();
switch(inChar){
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus(serial);
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
if(pinCounting){
// set the pin according to the pin Command
setPinCommand();
}
numCount = 0;
pinCount = 0;
break;
case 'V':
serial->println("SERVOTOR32_v2.02");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
serial->println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
serial->println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
changeServo(inServo, -1);
servoCounting = false;
}
if(pinCounting){
if(pinCount < 3){
pinCommand[pinCount] = inChar;
pinCount++;
}
}
break;
case 'S':
servoCounting = false;
posCounting = false;
pinCounting = true;
pinCount = 0;
break;
default:
if(pinCounting){
if(pinCount < 3){
pinCommand[pinCount] = inChar;
pinCount++;
}
}
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
