/* ***************************************************************
Author:  Thomas Braunl, 04.Nov.04 
Purpose: Drive an AI-Motor from the EyeBot's Serial Port
********************************************************************/

#include "eyebot.h"
#define PORT   SERIAL3
#define BAUD   SER57600
#define SERVOS  31
#define MIDDLE 125
#define DEBUG    1

// drive motor to angle
void DriveTo(BYTE motor, BYTE angle, BYTE speed)
{ BYTE valA, check, ret1, ret2;
  OSSendCharRS232(0xFF,  PORT); // Header
  valA = (speed<<5) | motor;
  OSSendCharRS232(valA,  PORT); // A: 3 bit speed (0=max, 4=min) + 4 bit ID
  OSSendCharRS232(angle, PORT); // B: position (0..254)
  check = (valA ^ angle) & 0x7F;
  OSSendCharRS232(check, PORT); // checksum (A XOR B) &0x7f
  
  OSRecvRS232(&ret1, PORT);     // 1: current
  OSRecvRS232(&ret2, PORT);     // 2: position
  if (DEBUG) LCDPrintf(">Cur %d Pos %d\n", ret1, ret2);
}

// drive multiple motors to angle
void DriveMultiple(BYTE number, BYTE angles[], BYTE speed)
{ BYTE valA, motor, check;
  OSSendCharRS232(0xFF,   PORT); // Header
  valA = (speed<<5) | 0x1F;
  OSSendCharRS232(valA,   PORT); // A: 3 bit speed (0=max, 4=min) + 11111
  OSSendCharRS232(number, PORT); // B: last ID
  check = 0;
  for (motor=0; motor<number; motor++)
  { OSSendCharRS232(angles[motor], PORT); // C: angle
    check ^= angles[motor];
  }
  check &= 0x7F;
  OSSendCharRS232(check, PORT); // checksum
  // no response packet
}


// power down motors
void PowerDown()
{ BYTE check, ret1, ret2;
  OSSendCharRS232(0xFF,  PORT); // Header
  OSSendCharRS232(0xFF, PORT); // Header
  OSSendCharRS232(0xDF, PORT); // A
  OSSendCharRS232(0x20, PORT); // B
  check = (0xDF ^ 0x20) & 0x7F;
  OSSendCharRS232(check, PORT); // checksum (A XOR B) &0x7f
  
  OSRecvRS232(&ret1, PORT);     // 1: don't care
  OSRecvRS232(&ret2, PORT);     // 2: don't care
}


// change ID of motor
void ChangeID(BYTE oldID, BYTE newID)
{ BYTE valA, check, ret1, ret2;
  OSSendCharRS232(0xFF,  PORT); // Header
  valA = 0x07 | oldID;
  OSSendCharRS232(valA,  PORT); // A: 7 or ID
  OSSendCharRS232(0x0A,  PORT);  // B: 0x0A
  OSSendCharRS232(newID, PORT);  // C: newID
  OSSendCharRS232(newID, PORT);  // D: newID
  check = (valA ^ 0x0A ^ newID ^ newID) & 0x7F;
  OSSendCharRS232(check, PORT); // checksum (A XOR B) &0x7f
  
  OSRecvRS232(&ret1, PORT);     // 1: current
  OSRecvRS232(&ret2, PORT);     // 2: position
  if (DEBUG) LCDPrintf(">ID %d ID %d\n", ret1, ret2);
}


int main ()
{ int i, err, key;
  BYTE motor, angles[SERVOS];
  
  LCDPutString("AI-Motor\n");
  LCDMenu("MOT","+","-","END");
  err = OSInitRS232(BAUD, NONE, PORT);
  if (err) LCDPrintf("error init rs232 %d\n", err);
  motor = 0;
  for (i=0; i<SERVOS; i++) angles[i]=MIDDLE;

  do
  { LCDPrintf("Mot:%2d Ang:%3d\n", motor, angles[motor]);
    switch (key = KEYGet())
    { case KEY1: motor++;
                 if (motor>=SERVOS) motor=0;
                 break;
      case KEY2: angles[motor] += 5;
                 if (angles[motor]>=255) angles[motor]=250;
                 DriveTo(motor, angles[motor], 0);
                 break;           
      case KEY3: angles[motor] -= 5;
                 if (angles[motor]>=250) angles[motor]=0;
                 DriveTo(motor, angles[motor], 0);
                 break;
      /* case KEY3: for (i=0; i<SERVOS; i++) angles[i]=MIDDLE;
                 DriveMultiple(SERVOS, angles, 0);
                 break; */
    }
  } while (key != KEY4);
  PowerDown();
  return 0;
}