Here is the new code. I got rid of delay() in my main program. In doing so I trimmed 36 bytes out of my program. However, I feel that using goto is not the most efficient way. The problem is, I can't think of a better way at the moment. Perhaps you can give me further suggestions? In the meantime, I need to also get rid of delay() in the MeccaBrain library and use a faster communication protocol. Then I can think about parallel processing for some of the harder tasks. Thank you all!
//
// Deep Think 0.0.0.2
//
// programmed by George Andrews
//
// NOTE: This program is designed for the Meccanoid G15 (combined
// with the 86Duino One). It is a part of the Deep Think Project
// (first version of the software for the project). The Deep Think
// Project is designed to advance the intelligence (as well as
// motion and structural capabilities) of humanoid robots. This
// version of the program is designed to do the following.
//
// 1. Perform various moves using functions.
// 2. Control the 2 D/C motors with the 86Duino.
//
// Please note that this version is only intended as a demo to
// establish a basic framework for interfacing the Meccanoid
// electronic parts with the 86Duino One for future development.
// This version of the software, however, has the following
// additional capabilities compared to the previous version Deep
// Think 0.0.0.1.
//
// 1. Can interface with the D/C motors as well as the servos and
// LED eyes.
// 2. Can perform various moves using functions in the program.
//
#include "MeccaBrain.h"
//
// Variables
//
// pin declarations
int LeftArmDaisyChainPin = 21;
int LEDEyePin = 29;
int RightArmDaisyChainPin = 38;
int LeftFootDCMotorPin = 11;
int RightFootDCMotorPin = 13;
// class objects
MeccaBrain LeftArmDaisyChain(LeftArmDaisyChainPin);
MeccaBrain LEDEye(LEDEyePin);
MeccaBrain RightArmDaisyChain(RightArmDaisyChainPin);
// timer variables
unsigned long TimeNeeded;
unsigned long LastTimeUpdate = millis();
//
// Functions
//
void WaveWithLeftArm()
{
// up
LeftArmDaisyChain.setServoPosition(0, 200);
LeftArmDaisyChain.communicate();
// let motors calibrate
TimeNeeded = 500;
WLAT1:
if (Time() == 1)
{
goto WLAT2;
}
else
{
goto WLAT1;
}
// down
WLAT2:
LeftArmDaisyChain.setServoPosition(0, 127);
LeftArmDaisyChain.communicate();
// let motors calibrate
WLAT3:
if (Time() == 1)
{
goto WLAT4;
}
else
{
goto WLAT3;
}
// up again
WLAT4:
LeftArmDaisyChain.setServoPosition(0, 200);
LeftArmDaisyChain.communicate();
// let motors calibrate
WLAT5:
if (Time() == 1)
{
goto WLAT6;
}
else
{
goto WLAT5;
}
// down all the way
WLAT6:
LeftArmDaisyChain.setServoPosition(0, 0);
LeftArmDaisyChain.communicate();
}
void WaveWithRightArm()
{
// up
RightArmDaisyChain.setServoPosition(0, 55);
RightArmDaisyChain.communicate();
// let motors calibrate
TimeNeeded = 500;
WRAT1:
if (Time() == 1)
{
goto WRAT2;
}
else
{
goto WRAT1;
}
// down
WRAT2:
RightArmDaisyChain.setServoPosition(0, 127);
RightArmDaisyChain.communicate();
// let motors calibrate
WRAT3:
if (Time() == 1)
{
goto WRAT4;
}
else
{
goto WRAT3;
}
// up again
WRAT4:
RightArmDaisyChain.setServoPosition(0, 55);
RightArmDaisyChain.communicate();
// let motors calibrate
WRAT5:
if (Time() == 1)
{
goto WRAT6;
}
else
{
goto WRAT5;
}
// down all the way
WRAT6:
RightArmDaisyChain.setServoPosition(0, 255);
RightArmDaisyChain.communicate();
}
void RaiseLeftHand()
{
LeftArmDaisyChain.setServoPosition(0, 200);
LeftArmDaisyChain.communicate();
}
void RaiseRightHand()
{
RightArmDaisyChain.setServoPosition(0, 55);
RightArmDaisyChain.communicate();
}
void PutDownLeftHand()
{
LeftArmDaisyChain.setServoPosition(0, 0);
LeftArmDaisyChain.communicate();
}
void PutDownRightHand()
{
RightArmDaisyChain.setServoPosition(0, 255);
RightArmDaisyChain.communicate();
}
void TurnLeft()
{
analogWrite(RightFootDCMotorPin, 255);
TimeNeeded = 3000;
TLT1:
if (Time() == 1)
{
// continue...
}
else
{
goto TLT1;
}
}
void TurnRight()
{
analogWrite(LeftFootDCMotorPin, 255);
TimeNeeded = 3000;
TRT1:
if (Time() == 1)
{
// continue...
}
else
{
goto TRT1;
}
}
void MoveForward()
{
analogWrite(LeftFootDCMotorPin, 255);
analogWrite(RightFootDCMotorPin, 255);
TimeNeeded = 3000;
MFT1:
if (Time() == 1)
{
// continue...
}
else
{
goto MFT1;
}
}
// bends at elbow with hand in front of robot
void ReceiveWithLeftHand()
{
LeftArmDaisyChain.setServoPosition(1, 255);
LeftArmDaisyChain.communicate();
}
// bends at elbow with hand in front of robot
void ReceiveWithRightHand()
{
RightArmDaisyChain.setServoPosition(1, 0);
RightArmDaisyChain.communicate();
}
// time without delay()
int Time()
{
unsigned long Interval = TimeNeeded;
unsigned long CurrentTime = millis();
if ((CurrentTime - LastTimeUpdate) > Interval)
{
LastTimeUpdate = CurrentTime;
// time is up
return 1;
}
else
{
// time is not yet up
return 0;
}
}
void setup()
{
// set up DC motors
pinMode(LeftFootDCMotorPin, OUTPUT);
pinMode(RightFootDCMotorPin, OUTPUT);
}
void loop()
{
T1a:
RaiseLeftHand();
RaiseRightHand();
TimeNeeded = 2000;
T1b:
if (Time() == 1)
{
// time is up; continue
goto T2a;
}
else
{
// go back and repeat check
goto T1b;
}
T2a:
PutDownLeftHand();
PutDownRightHand();
T2b:
if (Time() == 1)
{
goto T3a;
}
else
{
goto T2b;
}
T3a:
WaveWithLeftArm();
T3b:
if (Time() == 1)
{
goto T4a;
}
else
{
goto T3b;
}
T4a:
WaveWithRightArm();
T4b:
if (Time() == 1)
{
goto T5a;
}
else
{
goto T4b;
}
T5a:
TurnLeft();
TimeNeeded = 1000;
T5b:
if (Time() == 1)
{
goto T6a;
}
else
{
goto T5b;
}
T6a:
TurnRight();
T6b:
if (Time() == 1)
{
goto T7a;
}
else
{
goto T6b;
}
T7a:
TimeNeeded = 2000;
MoveForward();
T7b:
if (Time() == 1)
{
goto T8a;
}
else
{
goto T7b;
}
T8a:
ReceiveWithLeftHand();
T8b:
if (Time() == 1)
{
goto T9a;
}
else
{
goto T8b;
}
T9a:
PutDownLeftHand();
T9b:
if (Time() == 1)
{
goto T10a;
}
else
{
goto T9b;
}
T10a:
ReceiveWithRightHand();
T10b:
if (Time() == 1)
{
goto T11a;
}
else
{
goto T10b;
}
T11a:
PutDownRightHand();
T11b:
if (Time() == 1)
{
goto T1a;
}
else
{
goto T11b;
}
}