Deep Think Project - Artificial Intelligence Research Project

Community General Discussion
28

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. :slight_smile: 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;
  }
}