void CheckVoltages(void)
{
#ifdef AVS_PIN
uint16_t wVS = analogRead(AVS_PIN);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)AVAL_MIN) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
#elif defined(PACKET_MODE)
// Using Roboclaw in packet mode, so we can ask it for the
// motor voltages... Note: This requires us to send and receive
// stuff serially, so probably only do this every so often.
if ((millis()-g_ulCVTimeLast) > 1000)
{
// more than a second since we last tested
uint16_t wVS = RClaw.ReadMainBatteryVoltage(PACKETS_ADDRESS);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)VOLTAGE_MIN1) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
g_ulCVTimeLast = millis(); // remember last we did it.
}
#endif
}
void MoveServo(PWM::Pin *apin, uint16_t wNew, uint16_t wTime)
{
long lPWNS = apin->GetDutyNS(); // Pulse Width in NS
Serial.print((long)(wNew*1000), DEC);
Serial.print(" ");
Serial.print(lPWNS, DEC);
uint16_t cCycles = (wTime/20);
Serial.print(" ");
Serial.print(cCycles, DEC);
if (cCycles) {
long lDeltaNSCycle = (((long)wNew*1000) - lPWNS) / cCycles;
Serial.print(" ");
Serial.println(lDeltaNSCycle, DEC);
while (cCycles--) {
lPWNS += lDeltaNSCycle;
apin->SetDutyNS(lPWNS);
delay(20); // delay a bit
}
}
apin->SetDutyUS(wNew);
}
uint8_t doArmIK(boolean fCartesian, int sIKX, int sIKY, int sIKZ, int sIKGA)
{
int t;
int sol0;
uint8_t bRet = IKS_SUCCESS; // assume success
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("(");
Serial.print(sIKX, DEC);
Serial.print(",");
Serial.print(sIKY, DEC);
Serial.print(",");
Serial.print(sIKZ, DEC);
Serial.print(",");
Serial.print(sIKGA, DEC);
Serial.print(")=");
}
#endif
if (fCartesian) {
// first, make this a 2DOF problem... by solving base
sol0 = radToServo(atan2(sIKX,sIKY));
// remove gripper offset from base
t = sqrt(sq((long)sIKX)+sq((long)sIKY));
// BUGBUG... Not sure about G here
#define G 30
sol0 -= radToServo(atan2((G/2)-G_OFFSET,t));
}
else {
// We are in cylindrical mode, probably simply set t to the y we passed in...
t = sIKY;
#ifdef DEBUG
sol0 = 0;
#endif
}
// convert to sIKX/sIKZ plane, remove wrist, prepare to solve other DOF
float flGripRad = (float)(sIKGA)*3.14159/180.0;
long trueX = t - (long)((float)WristLength*cos(flGripRad));
long trueZ = sIKZ - BaseHeight - (long)((float)WristLength*sin(flGripRad));
long im = sqrt(sq(trueX)+sq(trueZ)); // length of imaginary arm
float q1 = atan2(trueZ,trueX); // angle between im and X axis
long d1 = sq(ShoulderLength) - sq(ElbowLength) + sq((long)im);
long d2 = 2*ShoulderLength*im;
float q2 = acos((float)d1/float(d2));
q1 = q1 + q2;
int sol1 = radToServo(q1-1.57);
d1 = sq(ShoulderLength)-sq((long)im)+sq(ElbowLength);
d2 = 2*ElbowLength*ShoulderLength;
q2 = acos((float)d1/(float)d2);
int sol2 = radToServo(3.14-q2);
// solve for wrist rotate
int sol3 = radToServo(3.2 + flGripRad - q1 - q2 );
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("<");
Serial.print(sol0, DEC);
Serial.print(",");
Serial.print(trueX, DEC);
Serial.print(",");
Serial.print(trueZ, DEC);
Serial.print(",");
Serial.print(sol1, DEC);
Serial.print(",");
Serial.print(sol2, DEC);
Serial.print(",");
Serial.print(sol3, DEC);
Serial.print(">");
}
#endif
// Lets calculate the actual servo values.
if (fCartesian) {
sBase = min(max(512 - sol0, BASE_MIN), BASE_MAX);
}
sShoulder = min(max(512 - sol1, SHOULDER_MIN), SHOULDER_MAX);
// Magic Number 819???
sElbow = min(max(819 - sol2, SHOULDER_MIN), SHOULDER_MAX);
#define Wrist_Offset 512
sWrist = min(max(Wrist_Offset + sol3, WRIST_MIN), WRIST_MAX);
// Remember our current IK positions
g_sIKX = sIKX;
g_sIKY = sIKY;
g_sIKZ = sIKZ;
g_sIKGA = sIKGA;
// Simple test im can not exceed the length of the Shoulder+Elbow joints...
if (im > (ShoulderLength + ElbowLength)) {
if (g_bIKStatus != IKS_ERROR) {
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.println("IK Error");
}
#endif
MSound(2, 50, 3000, 50, 3000);
}
bRet = IKS_ERROR;
}
else if(im > (ShoulderLength + ElbowLength-IK_FUDGE)) {
if (g_bIKStatus != IKS_WARNING) {
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.println("IK Warning");
}
#endif
MSound(1, 75, 2500);
}
bRet = IKS_WARNING;
}
return bRet;
}
//===================================================================================================
// MoveArmTo
//===================================================================================================
void MoveArmTo(int sBase, int sShoulder, int sElbow, int sWrist, int sWristRot, int sGrip, int wTime, boolean fWait) {
int sMaxDelta;
int sDelta;
// First make sure servos are not free...
if (g_fServosFree) {
g_fServosFree = false;
for(uint8_t i=1; i <= CNT_SERVOS; i++) {
TorqueOn(i);
}
bioloid.readPose();
}
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("[");
Serial.print(sBase, DEC);
Serial.print(" ");
Serial.print(sShoulder, DEC);
Serial.print(" ");
Serial.print(sElbow, DEC);
Serial.print(" ");
Serial.print(sWrist, DEC);
Serial.print(" ");
Serial.print(sWristRot, DEC);
Serial.print(" ");
Serial.print(sGrip, DEC);
Serial.println("]");
}
#endif
// Make sure the previous movement completed.
// Need to do it before setNextPos calls as this
// is used in the interpolating code...
while (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
delay(3);
}
// Also lets limit how fast the servos will move as to not get whiplash.
bioloid.setNextPoseByIndex(SID_BASE, sBase);
sMaxDelta = abs(bioloid.getNextPoseByIndex(SID_RSHOULDER) - sShoulder);
bioloid.setNextPoseByIndex(SID_RSHOULDER, sShoulder);
bioloid.setNextPoseByIndex(SID_LSHOULDER, 1024-sShoulder);
sDelta = abs(bioloid.getNextPoseByIndex(SID_RELBOW) - sElbow);
if (sDelta > sMaxDelta)
sMaxDelta = sDelta;
bioloid.setNextPoseByIndex(SID_RELBOW, sElbow);
bioloid.setNextPoseByIndex(SID_LELBOW, 1024-sElbow);
sDelta = abs(bioloid.getNextPoseByIndex(SID_WRIST) - sWrist);
if (sDelta > sMaxDelta)
sMaxDelta = sDelta;
bioloid.setNextPoseByIndex(SID_WRIST, sWrist);
#ifdef OPT_WRISTROT
bioloid.setNextPoseByIndex(SID_WRISTROT, sWristRot);
#endif
bioloid.setNextPoseByIndex(SID_GRIP, sGrip);
// Save away the current positions...
g_sBase = sBase;
g_sShoulder = sShoulder;
g_sElbow = sElbow;
g_sWrist = sWrist;
g_sWristRot = sWristRot;
g_sGrip = sGrip;
// Now start the move - But first make sure we don't move too fast.
if (((long)sMaxDelta*wTime/1000L) > MAX_SERVO_DELTA_PERSEC) {
wTime = ((long)sMaxDelta*1000L)/ MAX_SERVO_DELTA_PERSEC;
}
bioloid.interpolateSetup(wTime);
// Do at least the first movement
bioloid.interpolateStep();
// And if asked to, wait for the previous move to complete...
if (fWait) {
while (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
delay(3);
}
}
}
void FindServoZeroPoints()
{
// not clean but...
int data;
short sSN; // which servo number
boolean fNew = true; // is this a new servo to work with?
boolean fExit = false; // when to exit
int ich;
word wCenter;
// OK lets move all of the servos to their zero point.
InitializeServos();
Serial.println("Find Servo Zeros.\n$-Exit, +- changes, *-change servo");
// Lets move all of the servos to their default location...
for (sSN=0; sSN < MNUMSERVOS; sSN++)
g_aServos[sSN]->SetDutyUS(rcd.aServos[sSN].wCenter);
sSN = 0; // start at our first servo.
while(!fExit) {
if (fNew) {
wCenter = rcd.aServos[sSN].wCenter;
Serial.print("Servo: ");
Serial.print(g_apszServos[sSN]);
Serial.print("(");
Serial.print(wCenter-1500, DEC);
Serial.println(")");
// Now lets wiggle the servo
MoveServo(g_aServos[sSN], wCenter+250, 500);
MoveServo(g_aServos[sSN], wCenter-250, 500);
MoveServo(g_aServos[sSN], wCenter, 500);
fNew = false;
}
//get user entered data
data = Serial.read();
//if data received
if (data !=-1) {
if (data == '$')
fExit = true; // not sure how the keypad will map so give NL, CR, LF... all implies exit
else if ((data == '+') || (data == '-')) {
if (data == '+')
rcd.aServos[sSN].wCenter += 5; // increment by 5us
else
rcd.aServos[sSN].wCenter -= 5; // increment by 5us
Serial.print(" ");
Serial.println(rcd.aServos[sSN].wCenter, DEC);
// Lets try to use attach to change the offsets...
MoveServo(g_aServos[sSN], rcd.aServos[sSN].wCenter, 100);
} else if ((data >= '0') && (data < ('0'+ MNUMSERVOS))) {
// direct enter of which servo to change
fNew = true;
sSN = data - '0';
} else if (data == '*') {
// direct enter of which servo to change
fNew = true;
sSN++;
if (sSN == MNUMSERVOS)
sSN = 0;
}
}
}
Serial.print("Find Servo exit ");
for (sSN=0; sSN < MNUMSERVOS; sSN++){
Serial.print(" ");
Serial.print(g_aServos[sSN]->GetDutyNS()/1000, DEC);
}
Serial.print("\nSave Changes? Y/N: ");
//get user entered data
while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15)))
;
if ((data == 'Y') || (data == 'y')) {
// call off to save away the updated data.
rcd.Save();
} else {
g_fServosInit = false; // Lets go back and reinit...
rcd.Load();
}
}
//==============================================================================
// TerminalMonitor - Simple background task checks to see if the user is asking
// us to do anything, like update debug levels ore the like.
//==============================================================================
boolean TerminalMonitor(void)
{
uint8_t szCmdLine[20];
int ich;
int ch;
// See if we need to output a prompt.
if (g_fShowDebugPrompt) {
Serial.println("Rover Debug Monitor");
Serial.println("D - Toggle debug on or off");
#ifdef BBB_SERVO_SUPPORT
Serial.println("O - Enter Servo offset mode");
Serial.println("S <SN> <ANGLE> - Move Servo");
#endif
g_fShowDebugPrompt = false;
}
// First check to see if there is any characters to process.
if (ich = Serial.available()) {
ich = 0;
// For now assume we receive a packet of data from serial monitor, as the user has
// to click the send button...
for (ich=0; ich < sizeof(szCmdLine); ich++) {
ch = Serial.read(); // get the next character
if ((ch == -1) || ((ch >= 10) && (ch <= 15)))
break;
szCmdLine[ich] = ch;
}
szCmdLine[ich] = '\0'; // go ahead and null terminate it...
Serial.print("Serial Cmd Line:");
Serial.write(szCmdLine, ich);
Serial.println("!!!");
// So see what are command is.
if (ich == 0) {
g_fShowDebugPrompt = true;
} else if ((ich == 1) && ((szCmdLine[0] == 'd') || (szCmdLine[0] == 'D'))) {
g_fDebugOutput = !g_fDebugOutput;
if (g_fDebugOutput)
Serial.println("Debug is on");
else
Serial.println("Debug is off");
#ifdef BBB_SERVO_SUPPORT
} else if ((ich == 1) && ((szCmdLine[0] == 'o') || (szCmdLine[0] == 'O'))) {
FindServoZeroPoints();
} else if (((szCmdLine[0] == 's') || (szCmdLine[0] == 'S'))) {
// ok lets grab a servo number
int iServo = 0;
int iAngle = 0;
int i;
for (i=1; i< ich; i++) {
if (szCmdLine[i] != ' ')
break;
}
if ((szCmdLine[i] >= '0') && (szCmdLine[i] <= '9')) {
iServo = szCmdLine[i++] - '0';
// now angle
while (szCmdLine[i] == ' ')
i++;
while ((szCmdLine[i] >= '0') && (szCmdLine[i] <= '9')) {
iAngle = iAngle*10 + szCmdLine[i++] - '0';
}
// Ok lets try moving the servo there...
g_aServos[iServo]->SetDutyUS(iAngle);
}
#endif
}
return true;
}
return false;
}
//--------------------------------------------------------------------------
// Main: the main function.
//--------------------------------------------------------------------------
int main(int argc, char *argv[])
{
// Install signal handler to allow us to do some cleanup...
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = SignalHandler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
char abT[40]; // give a nice large buffer.
uint8_t cbRead;
printf("Start\n");
if (argc > 1)
{
for (int i=1; i < argc; i++)
{
printf("%d - %s\n", i, argv[i]);
}
}
char *pszDevice;
if (!RClaw.begin(pszDevice = (argc > 1? argv[1] : szRClawDevice), B38400))
{
printf("RClaw (%s) Begin failed\n", pszDevice);
return 0;
}
if (!command.begin(pszDevice = (argc > 2? argv[2] : szCommanderDevice), B38400))
{
printf("Commander (%s) Begin failed\n", pszDevice);
return 0;
}
int error;
delay(250);
Serial.begin(/*57600*/);
// Try to load the Rover Configuration Data
rcd.Load();
g_MotorsDriver.Init();
Serial.println("Kurt's Rover Program Startup\n");
g_fDebugOutput = false; // start with it off!
g_fShowDebugPrompt = true;
g_fRoverActive = false;
g_fRoverActivePrev = false;
g_fServosInit = false;
g_bGear = 3; // We init in 3rd gear.
g_bSteeringMode = ONE_STICK_MODE;
// Initialize our pan and tilt servos
InitializeServos(); // Make sure the servos are active
for(;;)
{
//--------------------------------------------------------------------------
// Loop: the main arduino main Loop function
//--------------------------------------------------------------------------
// We also have a simple debug monitor that allows us to
// check things. call it here..
if (TerminalMonitor())
continue;
CheckVoltages(); // check voltages - if we get too low shut down the servos...
// Lets get the PS2 data...
ControlInput();
// Drive the rover
if (g_fRoverActive) {
if (g_bSteeringMode == TANK_MODE) {
sRDrivePWM = LStickY; //RStickY; // BUGBUG - appears like wrong ones doing each...
sLDrivePWM = RStickY; // LStickY;
} else { // One stick driving
if ((RStickY >=0) && (RStickX >= 0)) { // Quadrant 1
sRDrivePWM = RStickY - RStickX;
sLDrivePWM = max(RStickX, RStickY);
} else if ((RStickY<0) && (RStickX>=0)) { //Quadrant 2
sRDrivePWM = (RStickY + RStickX);
sLDrivePWM = min (-RStickX, RStickY);
} else if ((RStickY<0) && (RStickX<0)) { //Quadrant 3
sRDrivePWM = min (RStickX, RStickY);
sLDrivePWM = (RStickY - RStickX);
} else if ((RStickY>=0) && (RStickX<0)) { // Quadrant 4
sRDrivePWM = max(-RStickX, RStickY);
sLDrivePWM = (RStickY + RStickX);
} else {
sRDrivePWM = 0;
sLDrivePWM = 0;
}
}
// Lets output the appropriate stuff to the motor controller
// ok lets figure out our speeds to output to the two motors. two different commands
// depending on going forward or backward.
// Scale the two values for the motors.
sRDrivePWM = max(min((sRDrivePWM * g_bGear) / 4, 127), -127); // This should keep up in the -127 to +127 range and scale it depending on what gear we are in.
sLDrivePWM = max(min((sLDrivePWM * g_bGear) / 4, 127), -127);
#ifdef DEBUG
if (g_fDebugOutput) {
if ((RStickY != RStickYPrev) || (RStickX != RStickXPrev) ||
(LStickY != LStickYPrev) || (LStickX != LStickXPrev) ||
(sRDrivePWM != sRDrivePWMPrev) || (sLDrivePWM != sLDrivePWMPrev)) {
Serial.print(LStickY, DEC);
Serial.print(",");
Serial.print(LStickX, DEC);
Serial.print(" ");
Serial.print(RStickY, DEC);
Serial.print(",");
Serial.print(RStickX, DEC);
Serial.print(" - ");
Serial.print(sLDrivePWM, DEC);
Serial.print(",");
Serial.println(sRDrivePWM, DEC);
LStickYPrev = LStickY;
LStickXPrev = LStickX;
RStickYPrev = RStickY;
RStickXPrev = RStickX;
sRDrivePWMPrev = sRDrivePWM;
sLDrivePWMPrev = sLDrivePWM;
}
}
#endif
// Call our motors driver code which may change depending on how we talk to the motors...
g_MotorsDriver.RDrive(sRDrivePWM);
g_MotorsDriver.LDrive(sLDrivePWM);
// Also if we have a pan/tilt lets update that as well..
#ifdef BBB_SERVO_SUPPORT
if (g_bSteeringMode != TANK_MODE) {
if (LStickX ) {
if (command.buttons & BUT_L6) { //modify which thing we are controlling depending on if L6 is down or not.
w = max(min(g_wRot + LStickX/8, rcd.aServos[RoverConfigData::ROTATE].wMax), rcd.aServos[RoverConfigData::ROTATE].wMin);
if (w != g_wRot) {
pinRot.SetDutyUS(w);
g_wRot = w;
}
} else {
w = max(min(g_wPan + LStickX/8, rcd.aServos[RoverConfigData::PAN].wMax), rcd.aServos[RoverConfigData::PAN].wMin);
if (w != g_wPan) {
pinPan.SetDutyUS(w);
g_wPan = w;
}
}
}
if (LStickY) {
w = max(min(g_wTilt + LStickY/8, rcd.aServos[RoverConfigData::TILT].wMax), rcd.aServos[RoverConfigData::TILT].wMin);
if (w != g_wTilt) {
pinTilt.SetDutyUS(w);
g_wTilt = w;
}
}
}
#endif
delay (10);
} else {
if (g_fRoverActivePrev) {
MSound( 3, 100, 2500, 80, 2250, 60, 2000);
g_MotorsDriver.RDrive(0);
g_MotorsDriver.LDrive(0);
}
delay (10);
}
g_fRoverActivePrev = g_fRoverActive;
}
}