Exemplo n.º 1
0
//===================================================================================================
// Setup 
//====================================================================================================
void setup() {
  // Lets initialize the Commander
  Serial.begin();
  
	if (!command.begin("/dev/ttyXBEE", B38400))
	{
		printf("Commander Begin failed\n");
		return;
	}    

  // Next initialize the Bioloid
  bioloid.poseSize = CNT_SERVOS;

  // Read in the current positions...
  printf("Before readPose\n");
    bioloid.readPose();
  printf("After readPose\n");
  for (int i=1; i <= CNT_SERVOS; i++) {
    Serial.println(dxl_read_word(i, AX_PRESENT_POSITION_L), DEC);
  }  
    
  // Start off to put arm to sleep...
  Serial.println("Kurt's Arm");

  PutArmToSleep();

  MSound(3, 60, 2000, 80, 2250, 100, 2500);

}
Exemplo n.º 2
0
int main()
{
    // 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);

    Serial.begin();

    setup();

    for(;;)
    {
        loop();
    }
}
Exemplo n.º 3
0
//--------------------------------------------------------------------------
// 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;
    }
}