/**

******************************************************************************

* @file uArmService.cpp

* @author David.Long

* @email xiaokun.long@ufactory.cc

* @date 2016-09-28

* @license GNU

* copyright(c) 2016 UFactory Team. All right reserved

******************************************************************************

*/

#include "uArm.h"

#include "uArmComm.h"

uArmService service;

uArmService::uArmService()

{

mRecordAddr = 0;

mReportInterval = 0;

mButtonServiceDisable = false;

mReportStartTime = millis();

mTickRecorderTime = millis();

}

void uArmService::setButtonService(bool on)

{

if (on)

{

mButtonServiceDisable = false;

}

else

{

mButtonServiceDisable = true;

}

}

void uArmService::setReportInterval(unsigned int interval)

{

mReportInterval = interval;

}

void uArmService::init()

{

}

void uArmService::recorderTick()

{

//sys led function detec every 0.05s-----------------------------------------------------------------

switch(mSysStatus)//every 0.125s per point

{

case SINGLE_PLAY_MODE:

if(play() == false)

{

mSysStatus = NORMAL_MODE;

mRecordAddr = 0;

}

break;

case LOOP_PLAY_MODE:

if(play() == false)

{

mRecordAddr = 0;

}

break;

case LEARNING_MODE:

case LEARNING_MODE_STOP:

if(record() == false)

{

mSysStatus = NORMAL_MODE;

mRecordAddr = 0;

controller.attachAllServo();

}

break;

default:

break;

}

}

void uArmService::systemRun()

{

//check the button4 status------------------------------------------------------------------------

if (mButtonServiceDisable)

{

if (buttonMenu.longPressed())

{

buttonMenu.clearEvent();

reportButtonEvent(0, EVENT_LONG_PRESS);

}

else if (buttonMenu.clicked())

{

//debugPrint("btnD4 down");

buttonMenu.clearEvent();

reportButtonEvent(0, EVENT_CLICK);

}

//check the button7 status-------------------------------------------------------------------------

if (buttonPlay.longPressed())

{

buttonPlay.clearEvent();

reportButtonEvent(1, EVENT_LONG_PRESS);

}

else if (buttonPlay.clicked())

{

buttonPlay.clearEvent();

reportButtonEvent(1, EVENT_CLICK);

}

}

else

{

if (buttonMenu.clicked())

{

//debugPrint("btnD4 down");

buttonMenu.clearEvent();

switch (mSysStatus)

{

case NORMAL_MODE:

case NORMAL_BT_CONNECTED_MODE:

mSysStatus = LEARNING_MODE;

mRecordAddr = 0;//recording/playing address

controller.detachAllServo();

break;

case LEARNING_MODE:

//LEARNING_MODE_STOP is just used to notificate record() function to stop, once record() get it then change the sys_status to normal_mode

mSysStatus = LEARNING_MODE_STOP;//do not detec if BT is connected here, will do it seperatly

pumpOff();

break;

default: break;

}

}

//check the button7 status-------------------------------------------------------------------------

if (buttonPlay.longPressed())

{

buttonPlay.clearEvent();

switch(mSysStatus)

{

case NORMAL_MODE:

case NORMAL_BT_CONNECTED_MODE:

mRecordAddr = 0;

mSysStatus = LOOP_PLAY_MODE;

break;

case SINGLE_PLAY_MODE:

case LOOP_PLAY_MODE:

break;

case LEARNING_MODE:

break;

}

}

else if (buttonPlay.clicked())

{

buttonPlay.clearEvent();

//debugPrint("btnD7 down");

switch(mSysStatus)

{

case NORMAL_MODE:

case NORMAL_BT_CONNECTED_MODE:

mRecordAddr = 0;//recording/playing address

mSysStatus = SINGLE_PLAY_MODE; // or play just one time

break;

case SINGLE_PLAY_MODE:

case LOOP_PLAY_MODE:

pumpOff();

mSysStatus = NORMAL_MODE;

break;

case LEARNING_MODE:

if (getPumpStatus())

{

pumpOff();

}

else

{

pumpOn();

}

break;

}

}

}

if (mReportInterval > 0)

{

if(millis() - mReportStartTime >= mReportInterval)

{

mReportStartTime = millis();

reportPos();

}

}

}

void uArmService::btDetect()

{

#ifdef MKII

if (!buzzer.on() && ((mSysStatus == NORMAL_MODE) || (mSysStatus == NORMAL_BT_CONNECTED_MODE)))

{

pinMode(BT_DETECT_PIN, INPUT);

digitalWrite(BT_DETECT_PIN,HIGH);

if (digitalRead(BT_DETECT_PIN) == HIGH)//do it here

{

ledRed.on();

mSysStatus = NORMAL_BT_CONNECTED_MODE;

}

else

{

ledRed.off();

mSysStatus = NORMAL_MODE;

}

//pinMode(BT_DETECT_PIN, OUTPUT);

}

#endif

}

void uArmService::run()

{

systemRun();

if (millis() - mTickRecorderTime >= 50)

{

mTickRecorderTime= millis();

recorderTick();

}

}

bool uArmService::play()

{

unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper

recorder.read(mRecordAddr, data, 5);

debugPrint("mRecordAddr = %d, data=%d, %d, %d", mRecordAddr, data[0], data[1], data[2]);

if(data[0] != 255)

{

//double x, y, z;

//controller.getXYZFromAngle(x, y, z, (double)data[2], (double)data[0], (double)data[1]);

//moveToAngle((double)data[2], (double)data[0], (double)data[1]);

controller.writeServoAngle((double)data[2], (double)data[0], (double)data[1]);

controller.writeServoAngle(SERVO_HAND_ROT_NUM, (double)data[3]);

unsigned char pumpStatus = getPumpStatus() > 0 ? 1 : 0;

if (pumpStatus != data[4])

{

if (data[4])

{

pumpOn();

}

else

{

pumpOff();

}

}

}

else

{

pumpOff();

return false;

}

mRecordAddr += 5;

return true;

}

bool uArmService::record()

{

debugPrint("mRecordAddr = %d", mRecordAddr);

if(mRecordAddr <= 65530)

{

unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper

if((mRecordAddr != 65530) && (mSysStatus != LEARNING_MODE_STOP))

{

double rot, left, right;

//controller.updateAllServoAngle();

controller.readServoAngles(rot, left, right);

data[0] = (unsigned char)left;

data[1] = (unsigned char)right;

data[2] = (unsigned char)rot;

data[3] = (unsigned char)controller.readServoAngle(SERVO_HAND_ROT_NUM);

data[4] = getPumpStatus() > 0 ? 1 : 0;

debugPrint("l=%d, r=%d, r= %d", data[0], data[1], data[2]);

}

else

{

data[0] = 255;//255 is the ending flag

recorder.write(mRecordAddr, data, 5);

return false;

}

recorder.write(mRecordAddr, data, 5);

mRecordAddr += 5;

return true;

}

else

{

return false;

}

}