// Program: AccelGimbal.nxc
// Author:  Alex Alspach (alexalspach@gmai.com)
// Date:    March 2009
// Status:  Working

#include "NXCDefs.h"

#define PROTO_PORT IN_1

#define   px 0.65
#define   py 0.65

#define   dx 0.06
#define   dy 0.06

#define   ix 0.01//0.01
#define   iy 0.01//0.01

#define   alphaX 0.5
#define   alphaY 0.5

int inputdata;
float A0;
float A1;
float A2;
float X;
float Y;
float Z;

float errorX;
float errorY;
float errorX_old;
float errorY_old;

float DerX;
float DerY;
float IntX = 0.0;
float IntY = 0.0;

float X_old;
float Y_old;

long motorX;
long motorY;

float tCalcStart;
float dT = 1.0;

int outputdata;
int count;
byte cmndbuf[];                 // buffer for outbound I2C command
byte respbuf[];                 // buffer for inbound I2C response

/* protoboard I/O map
   42,43 - A0 input
   44,45 - A1 input
   46,47 - A2 input
   48,49 - A3 input
   4A,4B - A4 input
   4C    - B inputs
   4D    - B outputs
   4E    - B controls
*/

void readA0()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x42;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A0=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
  
void readA1()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x44;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A1=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
  
void readA2()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x46;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A2=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
  
void writedata()
  {
  ArrayInit(cmndbuf, 0, 3);     // set the buffer to hold 3 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x4D;            // to set write address
  cmndbuf[2] = outputdata;      // to set write data
  count=0;                      // no bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  }

task main()
  {

  byte b0 = 1;
  byte b1 = 2;
  byte b2 = 4;
  byte b3 = 8;
  byte b4 = 16;
  byte b5 = 32;

  SetSensorLowspeed(PROTO_PORT); // set sensor port 1 to low speed serial (I2C)
  Wait(100);

  ArrayInit(cmndbuf, 0, 3);     // set the buffer to hold 3 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x4E;            // to set write address
  cmndbuf[2] = 0x3F;            // to write 111111
  count=0;                      // no bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command
  Wait(100);
  


  while (TRUE)
    {
    
    tCalcStart = CurrentTick();
    
    readA0();                   // read the analog port 0
    readA1();
    readA2();
    ClearScreen();
    
    // low pass filter
    // y[i] = y[i-1] + a * (x[i] - y[i-1])
    X = X_old + alphaX*(A2 - X_old) - 512/2;
    Y = Y_old + alphaY*(A1 - Y_old) - 512/2;

    Z = (A0 - 512);

    errorX = X;
    errorY = Y;
        
    DerX = (errorX-errorX_old)/dT;
    IntX = IntX + errorX*dT;
      
    DerY = (errorY-errorY_old)/dT;
    IntY = IntY + errorY*dT;
      
    motorX = px*X + dx*DerX + ix*IntX;
    motorY = py*Y + dy*DerY + iy*IntY;
    
    NumOut(20, LCD_LINE1, X);
    NumOut(20, LCD_LINE2, Y);
    NumOut(20, LCD_LINE3, Z);
    
    NumOut(20, LCD_LINE5, dT);

    // Limit the power to motor power range -100 to 100
    if (motorY > 100)   motorY = 100;
    if (motorY < -100)  motorY = -100;

    // Limit the power to motor power range -100 to 100
    if (motorX > 100)  motorX = 100;
    if (motorX < -100) motorX = -100;


    OnFwd(OUT_A, motorY);
    OnFwd(OUT_B, motorX);

    X_old = X;
    Y_old = Y;
    
    errorX_old = errorX;
    errorY_old = errorY;
    
    
    dT = (CurrentTick() - tCalcStart)/1000;
    //writedata();


    }
    
 }