wurm

/* wurm 00x
 Genetic-Algorithm-Based Autonomous Robot
 For Atmel ATTiny85
 @diemastermonkey 2016
 WORK IN PROGRESS
 */

/*   GA DNA Program code

 0  Do nothing                            // Outpin stuff
 2  Write from MemoryOut to OutPin
 4  Increment OutPin (w/wraparound)
 6  Decrement OutPin
 8  (Nothing...)

 10   Do nothing                            // Inpin stuff
 12  Read from InPin to MemoryIn
 14  Increment InPin (w/wraparound)
 16  Decrement InPin
 18  (Nothing...)

 20  Insert random value at MemoryIn       // Memory stuff
 21  Insert random value at MemoryOut
 22  Copy from MemoryIn to MemoryOut
 23  Copy from MemoryOut to MemoryIn
 24  Increment MemoryIn (w/wraparound)
 25  Increment MemoryOut
 26  Decrement MemoryIn
 27  Decrement MemoryOut
 28  Randomize MemoryIn
 29  Randomize MemoryOut
 */

#include <EEPROM.h>

/* GA "Memory"
 Each GA has a small buffer for integer data, and
 'MemoryIn/MemoryOut' indexes into the buffer. Commands
 can move the index, put data in or out of the buffer
 and read or write them pins.
 */

// New test smart servo/sensor code
int iServoCharge = -1;            // Set high then charge, set low on 0 then -1 to disable
int iServoPin = 2;                // For test only, GA must discover these things
int led = 3;                      // diags
int iContactPin = 1;              // Trigger switch or contact sensor pin

// Pins available to the GA
int iPins[] = {
  0, 1, 2, 3, 4, A1, A2, A3};
int iPinsMax = 7;          // Fast for frequent use

// GA: These store the GA *currently* under test
int iProgram[64];             // GAs entire program
int iProgramMax = 64;         // Hardwired for fast lookup
int iOutPin = 0;              // INDEX of iPins written to/read from
int iInPin = 0;
int iMemory[16];              // 16 bytes GA internal storage
int iMemoryIn = 0;            // INDEX where reading from memory
int iMemoryOut = 0;           // Where writing to memory
int iMemoryMax = 15;          // HARDWIRED limit of 'iMemory' for fast lookup

// The X genetic variants under test - copies of same structure
// They get copied into/out of "NowProgram" etc for tests/updates
int iGAProgram[3][64];
int iGAOutPin[3];
int iGAInPin[3];
int iGAMemory[3];
int iGAMemoryIn[3];
int iGAMemoryOut[3];

// For servo controls
int iPulseCycles = 32000;    // Total for 1 mhz clock
int iPulseHigh = 0;         // How many to keep pulse high
int iPulseLow = 0;          // For low (i.e. iPulseCycles - iPulseHigh)

// Setup run once on start
void setup() {
  pinMode (led, OUTPUT);
  pinMode (iServoPin, OUTPUT);

  // Eeprom tests
  int iRead = EEPROM.read (0);    // Read from eeprom position 0
  for (int i=0; i < iRead; i++) {  // Flash back number read
    digitalWrite (led, HIGH);
    delayMicroseconds (1000);
    digitalWrite (led, LOW);
  }
  EEPROM.write (0, iRead++);    // increment for next test

  digitalWrite (iServoPin, HIGH);       // Nicety - position servo ~ halfway
  delayMicroseconds (80);
  digitalWrite (iServoPin, LOW);
  delayMicroseconds (1024000);

  // Randseed
  randomSeed (analogRead (0));
}

// Main
void loop() {

  // Simple GA tests
  fnTest();          // Test the "current" GA

  // Crude health scoring (*needs* to be in-line, i.e. re-write testing routine)
  // crude light sense test
  pinMode (4, INPUT);            // Probably superfluous? Tho, GA might kludge em
  int iLight = 1024 - analogRead (4);  // A2);
  iLight = iLight / 256;
  pinMode (iContactPin, INPUT);    // Again mb superfluous mb not
  int iSwitch = digitalRead (iContactPin);
  if (iSwitch == HIGH) { iSwitch = 1; }

  // Score is light level w/huge bonus if switch pressed NOW
  int iScore = iLight + iSwitch;

  // Flash out score
  for (int i=0; i < iScore + 1; i++) {
    digitalWrite (led, HIGH);
    delayMicroseconds (300);
    digitalWrite (led, LOW);
    delayMicroseconds (100);
  }

  // To do: Comparison, selection, reproduction

  fnMutate ();       // Mutate it - no fitness yet so just a test

  // TO DO: Integrate working servo controls with GA
  //  fnServoTask (10 + random(160));
  fnServoProcess();

}                                          // end main loop

void fnTest () {
  for (int i=0; i < iProgramMax; i++) {
    // Simple runtime indicator
    if (iProgram[i] != 0) {
      // digitalWrite(led, HIGH);
      fnServoTask (60 + 200 * (iMemory[iMemoryOut]/255));
    }
    else {
      // digitalWrite(led, LOW);
    }

    // Handle each 'instruction' in the GA's program
    switch (iProgram[i]) {
    case 2:                 // Write from MemoryOut to OutPin
    case 3:
    case 8:
    case 9:
    case 10:  // Bonus instances!
      fnSmartWrite (iOutPin, iMemory[iMemoryOut]);
      break;

    case 4:                 // Increment OutPin (w/wraparound)
      iOutPin++;
      if (iOutPin > iPinsMax) {
        iOutPin = 0;
      }
      break;

    case 6:                 // Decrement OutPin
      iOutPin--;
      if (iOutPin < 0) {
        iOutPin = iPinsMax;
      }
      break;

    case 12:                //  Read from InPin to MemoryIn
    case 13:
    case 15:
    case 17:      // Bonus instances
      iMemory[iMemoryIn] = fnSmartRead (iInPin);
      break;

    case 14:               // Increment InPin (w/wraparound)
      iInPin++;
      if (iInPin > iPinsMax) {
        iInPin = 0;
      }
      break;

    case 16:               // Decrement InPin
      iInPin--;
      if (iInPin < 0) {
        iInPin = iPinsMax;
      }
      break;

    case 20:               // Insert random value at MemoryIn
      iMemory[iMemoryIn] = random(256);
      break;

    case 21:               // Insert random value at MemoryOut
      iMemory[iMemoryOut] = random(256);
      break;

    case 24:               // Increment MemoryIn (w/wraparound)
      iMemoryIn++;
      if (iMemoryIn > iMemoryMax) {
        iMemoryIn = 0;
      }
      break;

    case 25:               // Increment MemoryOut
      iMemoryOut++;
      if (iMemoryOut > iMemoryMax) {
        iMemoryOut = 0;
      }
      break;

    case 26:               // Decrement MemoryIn
      iMemoryIn--;
      if (iMemoryIn < 0) {
        iMemoryIn = iMemoryMax;
      }
      break;

    case 27:               // Decrement MemoryOut
      iMemoryOut--;
      if (iMemoryOut < 0) {
        iMemoryOut = iMemoryMax;
      }
      break;

    case 28:               // Randomize MemoryIn
      iMemoryIn = random(iMemoryMax);
      break;

      // case 29:               // Randomize MemoryOut
      //  iMemoryOut = random(iMemoryMax);
      //  break;

      // MANUAL KLUDGE SERVO TEST
    case 30:
    case 31:
      fnServoTask (10 + iMemory[iMemoryOut]);
      break;
    }                        // End switch/case

    // ---------------------------------------
    // TO DO - ACCUMULATE SCORE FROM ACTIVITIES
    //         FOR LATER USE IN EVALUATION
    // ---------------------------------------
    /*
          CODE HERE
     CODE HERE
     CODE HERE
     */
  }
}

// Evaluate Fitness

// Mutate the 'current' GA - Also for initial first-time mutation
void fnMutate () {
  int iMutation = 0;        // Which mutation
  // Subject some of the GAs program to semirandom mutations
  for (int i=0; i < iProgramMax; i++) {
    if (0 == random (1000)) {            // 1-in-1000 chance each gene
      iMutation = random (16);           // Which mutation (undefined = benign)

      switch (iMutation) {
      case 0:                         // Gene erased
        iProgram[i] = 0;
        break;

      case 1:                      // Gene inverted
        iProgram[i] = 256 - iProgram[i];
        break;

      case 2:                      // Gene incremented
        iProgram[i]++;
        break;

      case 3:                      // Decremented
        iProgram[i]--;
        break;

      case 4:     // Gene dupes preceeding gene (cancer?) - nerfed if @ 0th
        if (i > 0) {
          iProgram[i] = iProgram[i-1];
        }
        break;

        // Gene completely randomized
        // BEWARE HARDWIRED DIE SIDES to approx. program cmds
      case 5:
      case 6:
      case 7:
      case 8:
      case 9:
      case 10:
      case 11:
      case 12:
      case 13:
      case 14:
      case 15:
        iProgram[i] = random(32);
        break;
      }                              // End switch/case
    }                                // End if
  }                                  // End for
}                                    // End function


/* Utility functions */

// SmartWrite - Write arg data to arg pin (of pins array) by expected means
void fnSmartWrite (int iArgPinIndex, int iArgData) {
  // Handle servos with "ServoTask", first
  if (iPins[iArgPinIndex] == iServoPin) {
    fnServoTask (iArgData);
    return;
  }

  if (iArgPinIndex < 5) {                  // First 4 in array are digital
    if (iArgData < 128) {                  // Bottom half treated as low
      digitalWrite (iPins[iArgPinIndex], HIGH);
    }
    else {
      digitalWrite (iPins[iArgPinIndex], LOW);
    }
  }
  else {                                 // Higher pins treat as analog
    analogWrite (iPins[iArgPinIndex], iArgData);
  }
}

// SmartRead - Just like SmartWrite but reads, returns result
int fnSmartRead (int iArgPinIndex) {
  if (iArgPinIndex < 5) {
    return (digitalRead(iPins[iArgPinIndex]));
  }
  else {
    return (analogRead (iPins[iArgPinIndex]));
  }
}


// Init servo to go to destination i (60=0' 280=180
void fnServoTask (int i) {
  iServoCharge = i;      // Charge counter and send high to servo
  digitalWrite (iServoPin, HIGH);
  // KLUDGE -v
  delayMicroseconds (i);          // KLUDGE FIX TEST
  digitalWrite (iServoPin, LOW);
  delayMicroseconds(32000);          // KLUDGE FIX TEST
  // KLUDGE end pwm signal -^
}

// Process servo if necessary
void fnServoProcess () {
  if (iServoCharge == -1) {
    return;
  }      // Nothing to do
  iServoCharge--;                          // Decrement charge
  if (iServoCharge == 0) {
    digitalWrite (iServoPin, LOW);             // Discharged, send low to servo
    iServoCharge = -1;                     // Done, disable countdown
  }
}