Exemplo n.º 1
0
void setup(void) {
  Serial.println(F("Touch Paint!"));
  
  tft.begin();

  if (!ts.begin()) {
    Serial.println("Couldn't start touchscreen controller");
    while (1);
  }
  Serial.println("Touchscreen started");
  
  tft.fillScreen(ILI9341_BLACK);
  
  // make the color selection boxes
  tft.fillRect(0, 0, BOXSIZE, BOXSIZE, ILI9341_RED);
  tft.fillRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, ILI9341_YELLOW);
  tft.fillRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, ILI9341_GREEN);
  tft.fillRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, ILI9341_CYAN);
  tft.fillRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, ILI9341_BLUE);
  tft.fillRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, ILI9341_MAGENTA);
 
  // select the current color 'red'
  tft.drawRect(0, 0, BOXSIZE, BOXSIZE, ILI9341_WHITE);
  currentcolor = ILI9341_RED;
}
Exemplo n.º 2
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.º 3
0
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
}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
//===================================================================================================
// 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);
    }
  }
}
Exemplo n.º 7
0
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();
    }

}
Exemplo n.º 8
0
//==============================================================================
// 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;
}
Exemplo n.º 9
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;
    }
}