/* Monitor a 3 axis accelerometer and control servos to keep sensor level
   If arm is upright then keep sensor level above arm, if inverted keep level below arm
   Servos should not move out ouf range
   All values and bargraphs from 0G to 1G displayed on 4 line by 20 char LCD
   atmega328p is being used on nano arduino
   Version 0.1 RifRaf 17-2-2013 - first version
   Version 0.2 - revised the code to remove 150 odd lines */

#include "hardware.h"

// Initialise the hardware
void appInitHardware(void) {
    initHardware();
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
    rprintfInit(displayGetWriter(&display));
    rprintf("ADXL345Accelerometer");  //Setup LCD for chars that will not change
    return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

    rprintfInit(displayGetWriter(&display));
    //Check accelerometer and create value for X Y Z
    accelerometerRead(acceleration);
    int valX = acceleration.accelerometer.x_axis_mG;
    int valY = acceleration.accelerometer.y_axis_mG;
    int valZ = acceleration.accelerometer.z_axis_mG;

    if (valZ >=0) {  //Check to see if arm is upright
        DRIVE_SPEED speed=act_getSpeed(&servo2);
        int move =  valX/125; // Greater valX will make arm take a larger step towards level
        speed = speed + move;
        act_setSpeed(&servo2,speed);  //Move arm
    }

    if (valZ <=-1) {  //Check to see if arm is upside down
        DRIVE_SPEED speed=act_getSpeed(&servo2);
        int move =  -valX/125; // Greater valX will make arm take a larger step towards level
        speed = speed + move;
        act_setSpeed(&servo2,speed);  //Move arm
    }

    displayGoto(&display,0,1);  // Print valX to LCD
    rprintf("X %d mG  ",valX);
        if (valX < 0) {
         valX = -valX;
    }
    displayHorizGraph(&display,10,1,valX,1000,10);

    DRIVE_SPEED speed=act_getSpeed(&servo3);
    int move =  valY/125; // Greater valX will make arm take a larger step towards level
    speed += move * ((valZ > 0) - (valZ < 0));
    act_setSpeed(&servo3,speed);  //Move arm


    displayGoto(&display,0,2);  //Display valY to LCD
    rprintf("Y %d mG  ",valY);
        if (valY < 0) {
         valY = -valY;
    }
    displayHorizGraph(&display,10,2,valY,1000,10);

    displayGoto(&display,0,3);  //Display valZ to LCD
    rprintf("Z %d mG  ",valZ);
        if (valZ < 0) {
         valZ = -valZ;
    }
    displayHorizGraph(&display,10,3,valZ,1000,10);

    return 0;
}