/*
 *  simple_ax12.cpp
 *  Bioloid
 *
 *  Created by Markus Schneider on 4/10/08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 */

#include <stdio.h>
#include "serialport_d2xx.h"
#include "serialport_ftdi.h"
#include "connection.h"
#include "packet.h"
#include "ax12.h"

int main (int argc, char * const argv[]) 
{
  int status;
  
  // create serial port
  SerialPort *port = new SerialPort_FTDI();
  
  // init connection
  Connection conn(port);  
  
  // Get a new instance of AX12 with id 9
  // all packets should be transferd over the connection
  // initialized before.
  AX12 *m9 = new AX12(9, &conn);
  
  // get present position of ax12
  word position;
  status = m9->getPresentPosition(&position);
  
  // check for errors
  if( status < 0 ){
    fprintf(stderr, "[ERROR]: %d", status);
    return -1;
  }
  
  // print present position
  printf("Position of AX12 with id %d is %d", m9->getId(), position);
  
  // move to position 0x200 with speed 0x50
  status = m9->setGoalPositionSpeed(0x200, 0x50);
  
  // check for errors
  if( status < 0 ){
    fprintf(stderr, "[ERROR]: %d", status);
    return -1;
  }
  
  delete port;
  delete m9;
  
  return 0;
}

