/* ***************************************************************
Author: Thomas Braunl, 04.Nov.04 for Andy2 (digital motors)
        after a program written for Johnny Walker by TB on 22.05.98

Purpose:
  The robot initializes in a balanced upright position.
  When pressing "go", the robot does squats where the robot bends 
  the hips, knees, ankles, and arms.
********************************************************************/

#include "eyebot.h"
#define PORT   SERIAL3
#define BAUD   SER57600
#define SERVOS 17  // number of servos
#define SPEED   3
#define DEBUG   1
#define ANGLE  40
#define WTIME 400

typedef enum{ lHipS, lHipB, lKnee, lAnkleB, lAnkleS,
              rHipS, rHipB, rKnee, rAnkleB, rAnkleS,
              lShoulderR, lShoulderB, lEllbow,
              rShoulderR, rShoulderB, rEllbow,  HeadT } MotNames;      


// 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);
}


// read motor angle
void DriveRead(BYTE motor, BYTE *angle, BYTE *current)
{ BYTE valA, check;
  OSSendCharRS232(0xFF,  PORT);  // Header
  valA = 0xA0 | motor;
  OSSendCharRS232(valA,  PORT);  // A: 101 + ID
  OSSendCharRS232(0x00,  PORT);  // B: arbitrary
  check = valA & 0x7F;
  OSSendCharRS232(check, PORT);  // checksum

  OSRecvRS232(current,   PORT);  // 1: current
  OSRecvRS232(angle,     PORT);  // 2: position
}


//read all motor angles
void DriveReadAll(BYTE number, BYTE angles[])
{ BYTE i, dummy;
  for (i=0; i<number; i++) DriveRead(i, &angles[i], &dummy);
}


//drive multiple motors
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: num. of motors - not last ID (doc err)
  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(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
}


// Record current robot position
void Record(char* txt, BYTE *now)
{ LCDClear();
  LCDPrintf("%s\n", txt);
  LCDMenu(" "," "," ","REC");
  while (KEYRead() != KEY4)
  { DriveReadAll(SERVOS, now);
    LCDSetPos(1,0);
    LCDPrintf("%3d     %3d\n", now[rHipS], now[lHipS]);
    LCDPrintf("%3d     %3d\n", now[rHipB], now[lHipB]);
    LCDPrintf("%3d     %3d\n", now[rKnee], now[lKnee]);
    LCDPrintf("%3d     %3d\n", now[rAnkleB], now[lAnkleB]);
    LCDPrintf("%3d     %3d\n", now[rAnkleS], now[lAnkleS]);
  }
}


int main ()
{ int err;
  BYTE down[SERVOS], up[SERVOS];
  /*     {123, 143, 248, 27, 154,   125, 141, 242, 30, 208,
        140, 195, 125,           115, 195, 130,    125}; */
  
  LCDPutString("This is Andy-II\n");
  LCDMenu("+","-"," ","GO");
  err = OSInitRS232(BAUD, NONE, PORT);
  if (err) LCDPrintf("error init rs232 %d\n", err);

/*  // set positions
  for (i=0; i<SERVOS; i++) down[i] = up[i]; // copy position
  down[lHipB]      += ANGLE;
  down[rHipB]      += ANGLE;
  down[lKnee]      += 2*ANGLE;
  down[rKnee]      += 2*ANGLE;
  down[lAnkleB]    -= ANGLE;
  down[rAnkleB]    -= ANGLE;
  down[lShoulderR] += 2*ANGLE;
  down[rShoulderR] -= 2*ANGLE; */
  
  Record("UP Position",   up);
  Record("DOWN Position", down);
  
  LCDMenu("GO"," "," "," ");
  KEYWait(KEY1);
  LCDMenu("GO"," "," ","END");
  while (KEYRead() != KEY4)             // repeat until key press
  { LCDPrintf("going up\n");
    DriveMultiple(SERVOS, up,   SPEED); // move to down position
    OSWait(WTIME);
    if (KEYRead() == KEY4) break;
    LCDPrintf("going down\n");
    DriveMultiple(SERVOS, down, SPEED); // move to up position
    OSWait(WTIME);
  }
  PowerDown();
  return 0;
}