//============================================================================== // MotorsDriver::RDrive //============================================================================== void MotorsDriver::LDrive(short sVal) { InitializeServos(); // make sure the PWM has been initialized. if (sVal != LDrivePrev) { LDrivePrev = sVal; g_aServos[MOTOR2_I].write(90+(sVal*2)/3); } }
Minibot::Minibot(Servo *one, Servo *two, SmartJoystick *rJoy, SmartJoystick *opJoy, Arm *arm, Pincer *pincer) { firstStageServo = one; secondStageServo = two; rightJoystick = rJoy; operatorJoystick = opJoy; stageOneDeployed = false; stageTwoDeployed = false; _arm = arm; _pincer = pincer; InitializeServos(); }
void InitBot(void) { //Call initialization functions. LockoutProtection(); InitializeMCU(); initUART(); InitializeEncoders(true,false); PresetEncoderCounts(0,0); InitializeMotors(true,true); InitializeServos(); InitializeLineSensor(); SetDischargeTime(350); //Modified for 4 ADC Ports (assuming all are IR). initADCPorts(); //Initialize state variables. angle = 90; shooting = false; rotate = rotate_time; move = move_time; IRtrig = false; lose_obj = false; //Pin PA3 is James. 0xff is off; 0x00 is on. GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_3); GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_3, 0xff); //Pin PA2 is the trigger; set the pin to perform weak pull-up (not open-drain). SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); GPIOPinTypeGPIOInput(GPIO_PORTA_BASE, GPIO_PIN_2); GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_2,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU); //Setup an interrupt on pin PA2 (flipPancake). IntEnable(INT_GPIOA); GPIOIntTypeSet(GPIO_PORTA_BASE, GPIO_PIN_2,GPIO_FALLING_EDGE); GPIOPinIntEnable(GPIO_PORTA_BASE, GPIO_PIN_2); }
int main(void){ char i; unsigned char data[16]; short wiichuck[7], xinit=0, yinit=0, l_vel, r_vel; int xpow, ypow; LockoutProtection(); InitializeMCU(); InitializeUART(); InitializeI2C(); InitializeServos(); SetServoPosition(SERVO_0, 140); InitializeMotors(true, false); InitializeEncoders(true, false); // UARTprintf("Initializing Nunchuck\n\n"); // I2CSend(0x52<<1, 2, 0x40, 0x00); // Wait(25); init_nunchuck(); // Wireless Nunchucks Zero @ 128 xinit = yinit = 128; while(1){ //Start Recalculating Values Wait(1); I2CSend(0x52<<1, 1, 0x00); Wait(1); I2CSend(0x52<<1, 1, 0x00); Wait(1); I2CSend(0x52<<1, 1, 0x00); if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){ UARTprintf("Send Zero Error:\n"); switch(I2CMasterErr(I2C0_MASTER_BASE)){ case I2C_MASTER_ERR_ADDR_ACK: UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n"); break; case I2C_MASTER_ERR_DATA_ACK: UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n"); break; case I2C_MASTER_ERR_ARB_LOST: UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n"); break; default: UARTprintf("WTF: %d\n", I2CMasterErr(I2C0_MASTER_BASE)); } // Reinitialize Nunchuck on error init_nunchuck(); }else{ Wait(1); I2CRecieve(0x52<<1, data, 6); // Nunchuck data is 6 bytes, but for whatever reason, MEMOREX Wireless Nunchuck wants to send 8... if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){ UARTprintf("Send Zero Error:\n"); switch(I2CMasterErr(I2C0_MASTER_BASE)){ case I2C_MASTER_ERR_ADDR_ACK: UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n"); break; case I2C_MASTER_ERR_DATA_ACK: UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n"); break; case I2C_MASTER_ERR_ARB_LOST: UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n"); break; } // Reinitialize Nunchuck on error init_nunchuck(); }else{ //for(i=0; i<6; i++) // data[i] = (data[i] ^ 0x17) + 0x17; // Nintendo decided to encrypt thir data... // Save Joystick Data wiichuck[0] = data[1]; // X Axis Joystick wiichuck[1] = data[0]; // Y Axis Joystick wiichuck[2] = (((unsigned short) data[2]) << 2) + (((unsigned short) data[5]) & (3<<2)); // X Axis Accel wiichuck[3] = (((unsigned short) data[3]) << 2) + (((unsigned short) data[5]) & (3<<4)); // Y Axis Accel wiichuck[4] = (((unsigned short) data[4]) << 2) + (((unsigned short) data[5]) & (3<<6)); // Z Axis Accel wiichuck[5] = data[5] & (1 << 1) ? 0 : 1; //'C' Button wiichuck[6] = data[5] & (1 << 0) ? 0 : 1; //'Z' Button //if (xinit == 0 && yinit == 0){ // xinit = wiichuck[0]-127; // yinit = wiichuck[1]-127; //}else{ xpow = (wiichuck[0]-xinit)/2; ypow = (wiichuck[1]-yinit)/2; l_vel = (xpow - ypow)*2; r_vel = (xpow + ypow)*2; l_vel = l_vel > 127 ? 127 : l_vel; r_vel = r_vel > 127 ? 127 : r_vel; l_vel = l_vel < -127 ? -127 : l_vel; r_vel = r_vel < -127 ? -127 : r_vel; //UARTprintf("X: %d\tY: %d\n", xpow*2, ypow*2); SetMotorPowers(l_vel / (wiichuck[5]==0 ? 2 : 1), r_vel / (wiichuck[5]==0 ? 2 : 1)); UARTprintf("Motor L: %d\tMotor R: %d\n", l_vel, r_vel); SetServoPosition(SERVO_0, wiichuck[6]==1 ? 255 : 140); UARTprintf("Nunchuck Data:\n"); for(i=0; i<7; i++){ UARTprintf(" %d\n", wiichuck[i]); }NL; Wait(100); } } } }
void initServo(void) { InitializeServos(); }
//-----------------------Private functions-----------------------------// int main(void) { uint16_t i; for( i = 0; i < 64000; i++) asm("nop"); InitializeClock(); InitializeSysTick(); #ifdef _USE_DISPLAY InitializeDisplay(); #endif #ifdef _USE_MOTORS InitializeMotors(); #endif #ifdef _USE_ADC_BATTERY InitializeBattery(); #endif #ifdef _USE_ADC_SHARP InitializeSharp(); #endif #ifdef _USE_ENCODERS InitializeEncoders(); #endif #ifdef _USE_SERVOS InitializeServos(); #endif #ifdef _USE_LED_NUCLEO InitializeLedNucleo(); #endif #ifdef _USE_LED_14 InitializeLed14(); #endif #ifdef _USE_LED_EYE InitializeLedEye(); #endif #ifdef _USE_BT InitializeBT(); #endif #ifdef _USE_WIFI InitializeWifi(); #endif #ifdef _USE_MPU InitializeMPU(); #endif #if 0 if(//MPU error)) { //sth bad had happened... } #endif oMotors.SetSpeedLeft( 0.0f ); oMotors.SetSpeedRight( 0.0f ); oServos.SetAngleArmLeft( 0.0f ); oServos.SetAngleArmRight( 0.0f ); oServos.SetAngleCamVer( 0.0f ); oServos.SetAngleCamHor( -15.0f ); while (1) { } return 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(); } }
//-------------------------------------------------------------------------- // 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; } }