//==============================================================================
// 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;
}
}
