int main() {
// Optical__value_all
u8 Optical_value_all;
// Optical__value_signal
u8 Optical_value_signal;
int Speed, Dir;
float Ultrasonic_value_all[3];
int i;
init_platform();
OptInit();
MotorInit();
BluetoothInit();
while (1) {
Optical_value_all = OptGetAll();
printf("optical_all : %X \r\n", Optical_value_all);
OptGetSingle(Opt1, &Optical_value_signal);
printf("optical_CH1 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt2, &Optical_value_signal);
printf("optical_CH2 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt3, &Optical_value_signal);
printf("optical_CH3 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt4, &Optical_value_signal);
printf("optical_CH4 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt5, &Optical_value_signal);
printf("optical_CH5 : %X \r\n\r\n", Optical_value_signal);
UltraGetAll(Ultrasonic_value_all);
for (i = 0; i < 3; i++) {
printf("u%d : %f mm\r\n", i, Ultrasonic_value_all[i]);
usleep(1000);
}
printf("\r\n");
SetMotorSpeed(MotorL, 30);
SetMotorSpeed(MotorR, -80);
Speed = GetMotorSpeed(MotorL);
printf("MotorL Speed : %d \r\n", Speed);
Speed = GetMotorSpeed(MotorR);
printf("MotorR Speed : %d \r\n", Speed);
Dir = GetMotorDir(MotorL);
printf("MotorL Dir : %d \r\n", Dir);
Dir = GetMotorDir(MotorR);
printf("MotorR Dir : %d \r\n\r\n", Dir);
usleep(50 * 1000);
BluetoothSend("Hello World\r\n",11);
}
cleanup_platform();
return 0;
}
void CJoystick::StopMotors()
{
MOTORSTATEMAP::iterator motorIterator = m_MotorState.begin();
for( ; motorIterator != m_MotorState.end(); motorIterator++ )
{
SetMotorSpeed( motorIterator->first, 0 );
}
}
void WinchControl::Update(double delta)
{
// Handle the stick inputs
m_speed = 0.0f;
if (m_joystick->IsDown(XBOX_360_BUTTON_BACK) || m_joystick->IsDown(XBOX_360_BUTTON_Y))
m_speed = -1.0f;
if (m_joystick->IsDown(XBOX_360_BUTTON_START) || m_joystick->IsDown(XBOX_360_BUTTON_A))
m_speed = 1.0f;
// Set the output
SetMotorSpeed();
}
void BallControl::Update(double delta)
{
// Handle the inputs
m_speed = 0.0f;
if (m_joystick->IsDown(XBOX_360_BUTTON_X))
m_speed = 1.0f;
if (m_joystick->IsDown(XBOX_360_BUTTON_B))
m_speed = -1.0f;
// Set the output
SetMotorSpeed();
}
void Sweeper::Initialize(Preferences *prefs) {
printf("2135: Sweeper Initialize\n");
//Initialize and read file
// Ensure Talons are in proper coast/brake mode
motorAcquire->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Coast);
indexerAcquire->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Brake);
// Initialize motor speeds
m_sweeperSpeed = Robot::LoadPreferencesVariable("SweepSpeed", 0.8);
SmartDashboard::PutNumber("SweepSpeed", m_sweeperSpeed );
SetMotorSpeed(0); // Sweeper motor off
SetIndexerSpeed(0); // Indexer motor off
}
BipedDef::BipedDef()
{
SetMotorTorque(2.0f);
SetMotorSpeed(0.0f);
SetDensity(20.0f);
SetRestitution(0.0f);
SetLinearDamping(0.0f);
SetAngularDamping(0.005f);
SetGroupIndex(--count);
EnableMotor();
EnableLimit();
DefaultVertices();
DefaultPositions();
DefaultJoints();
LFootPoly.friction = RFootPoly.friction = 0.85f;
}
void CInputManager::_Test()
{
short stickLeft = GetLeftStickX(0);
USHORT left = 0;
USHORT right = 0;
if (stickLeft < 0)
left = -stickLeft * 2;
else
right = stickLeft * 2;
SetMotorSpeed(0, left, right);
if (IsButtonDown(0, Buttons::A))
DebugManager->Debug(L"A", L"A");
if (IsButtonDown(0, Buttons::B))
DebugManager->Debug(L"B", L"B");
if (IsButtonDown(0, Buttons::Back))
DebugManager->Debug(L"Back", L"Back");
if (IsButtonDown(0, Buttons::Down))
DebugManager->Debug(L"Down", L"Down");
if (IsButtonDown(0, Buttons::LB))
DebugManager->Debug(L"LB", L"LB");
if (IsButtonDown(0, Buttons::Left))
DebugManager->Debug(L"Left", L"Left");
if (IsButtonDown(0, Buttons::LeftStick))
DebugManager->Debug(L"LeftStick", L"LeftStick");
if (IsButtonDown(0, Buttons::RB))
DebugManager->Debug(L"RB", L"RB");
if (IsButtonDown(0, Buttons::Right))
DebugManager->Debug(L"Right", L"Right");
if (IsButtonDown(0, Buttons::RightStick))
DebugManager->Debug(L"RightStick", L"RightStick");
if (IsButtonDown(0, Buttons::Start))
DebugManager->Debug(L"Start", L"Start");
if (IsButtonDown(0, Buttons::Up))
DebugManager->Debug(L"Up", L"Up");
if (IsButtonDown(0, Buttons::X))
DebugManager->Debug(L"X", L"X");
if (IsButtonDown(0, Buttons::Y))
DebugManager->Debug(L"Y", L"Y");
DebugManager->Debug(GetLeftTrigger(0), L"left trigger");
DebugManager->Debug(GetRightTrigger(0), L"right trigger");
DebugManager->Debug(GetLeftStickX(0), L"left stick x");
DebugManager->Debug(GetLeftStickY(0), L"left stick y");
DebugManager->Debug(GetRightStickX(0), L"right stick x");
DebugManager->Debug(GetRightStickY(0), L"right stick y");
DebugManager->Debug(GetLeftTriggerDelta(0), L"left trigger Delta");
DebugManager->Debug(GetRightTriggerDelta(0), L"right trigger Delta");
DebugManager->Debug(GetLeftStickXDelta(0), L"left stick x Delta");
DebugManager->Debug(GetLeftStickYDelta(0), L"left stick y Delta");
DebugManager->Debug(GetRightStickXDelta(0), L"right stick x Delta");
DebugManager->Debug(GetRightStickYDelta(0), L"right stick y Delta");
if (IsButtonPressed(0, Buttons::A))
printf("A pressed\n");
if (IsButtonPressed(0, Buttons::B))
printf("B pressed\n");
if (IsButtonPressed(0, Buttons::Back))
printf("Back pressed\n");
if (IsButtonPressed(0, Buttons::Down))
printf("Down pressed\n");
if (IsButtonPressed(0, Buttons::LB))
printf("LB pressed\n");
if (IsButtonPressed(0, Buttons::Left))
printf("Left pressed\n");
if (IsButtonPressed(0, Buttons::LeftStick))
printf("LeftStick pressed\n");
if (IsButtonPressed(0, Buttons::RB))
printf("RB pressed\n");
if (IsButtonPressed(0, Buttons::Right))
printf("Right pressed\n");
if (IsButtonPressed(0, Buttons::RightStick))
printf("RightStick pressed\n");
if (IsButtonPressed(0, Buttons::Start))
printf("Start pressed\n");
if (IsButtonPressed(0, Buttons::Up))
printf("Up pressed\n");
if (IsButtonPressed(0, Buttons::X))
printf("X pressed\n");
if (IsButtonPressed(0, Buttons::Y))
printf("Y pressed\n");
if (IsButtonReleased(0, Buttons::A))
printf("A released\n");
if (IsButtonReleased(0, Buttons::B))
printf("B released\n");
if (IsButtonReleased(0, Buttons::Back))
printf("Back released\n");
if (IsButtonReleased(0, Buttons::Down))
printf("Down released\n");
if (IsButtonReleased(0, Buttons::LB))
printf("LB released\n");
if (IsButtonReleased(0, Buttons::Left))
printf("Left released\n");
if (IsButtonReleased(0, Buttons::LeftStick))
printf("LeftStick released\n");
if (IsButtonReleased(0, Buttons::RB))
printf("RB released\n");
if (IsButtonReleased(0, Buttons::Right))
printf("Right released\n");
if (IsButtonReleased(0, Buttons::RightStick))
printf("RightStick released\n");
if (IsButtonReleased(0, Buttons::Start))
printf("Start released\n");
if (IsButtonReleased(0, Buttons::Up))
printf("Up released\n");
if (IsButtonReleased(0, Buttons::X))
printf("X released\n");
if (IsButtonReleased(0, Buttons::Y))
printf("Y released\n");
if (GamepadConnected())
printf("gamepad connected\n");
if (GamepadDisconnected())
printf("gamepad disconnected\n");
UINT connectedGamepads = 0;
for (UINT i = 0; i < XUSER_MAX_COUNT; i++)
if (IsGamepadActive(i))
connectedGamepads++;
DebugManager->Debug((int)connectedGamepads, L"Connected gamepads");
}
int main(void)
{
int i = 0;
for(; i<8; i++) {
sensors[i] = 0;
}
//Needs to be before sei.. don't ask why..
//Also needs a delay from start up..
_delay_ms(1000);
InitLCD();
sei();
SETUP_SENSOR_IO();
CLEAR_BIT(DDRE, PE5);//Input pin
ENABLE_EXTERNAL_INTERRUPT(INT5);//External Interrupt Request 5 INT5 enabled*/
InitServo(0);
InitMotor();
SetupTachometer();
float currentDirection = 0;
float targetDirection = 0;
bool terminated = false;
currentSpeed = 10;
tachoPulsesPerSecond = 0;
for (;;)
{
if(updateScreen) {
updateScreen = false;
ClearScreen();
WriteText("State:",1);
WriteText_StartingFrom(status,2,7);
WriteText("Lap:",3);
WriteText_StartingFrom(lap,4,7);
WriteText("Ticker:",5);
WriteText_StartingFrom(time,6,7);
WriteText("cycle:",7);
WriteText_StartingFrom(cyc,8,7);
}
//Terminated when lapCounter > 3
if(!terminated) {
float direction = GetFilteredSensorValue();
if (lapCounter > 3) {
SetMotorSpeed(0);
MoveServo(0);
terminated = false;
status = "Terminated";
updateScreen = true;
driveFlag = false;
lapCounter = 0;
continue;
}
targetDirection = direction * 45.0;
if (absFloat(targetDirection) >= absFloat(currentDirection)) {
currentDirection = targetDirection;
}
else {
currentDirection += (targetDirection - currentDirection);
}
MoveServo(currentDirection);
if (driveFlag) {
//Calculate new motor speed and accelerate/deccelerate
float desiredPulsePerSecond = 9 + (1.0 - absFloat(direction)) * 5;
float difference = desiredPulsePerSecond - tachoPulsesPerSecond;
float coefficient = 0.015;
if (difference < 0) {
coefficient = -0.015;
}
currentSpeed += 20 * coefficient;
if(currentSpeed>80) {
currentSpeed = 80;
}
if(currentSpeed<0) {
currentSpeed = 0;
}
SetMotorSpeed(currentSpeed);
}
}
}
return 0;
}
int main (void)
{
// speed_t wrist;
// speed_t finger;
uint16_t fingerPosition;
// initialize the hardware stuff we use
InitTimer ();
ADC_Init (ADC_PRESCALAR_AUTO);
InitMotors ();
InitTimerUART ();
// set the LED port for output
LED_DDR |= LED_MASK;
// Flash the LED for 1/2 a second...
ledOn ();
ms_spin (500);
ledOff ();
Log ("*****\n");
Log ("***** Bioloid Gripper Test\n");
Log ("***** Copyright 2007 HUVrobotics\n");
Log ("*****\n");
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
fingerPosition = ADC_Read (7);
Log ("Speed = 0... Position = %4d\n", fingerPosition);
SetMotorSpeed (SPEED_OFF, 0);
while (fingerPosition < MAX_FINGER)
{
fingerPosition = ADC_Read (7);
}
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
ms_spin (1000);
fingerPosition = ADC_Read (7);
Log ("Speed = 255... Position = %4d\n", fingerPosition);
SetMotorSpeed (SPEED_OFF, 255);
while (fingerPosition > MIN_FINGER)
{
fingerPosition = ADC_Read (7);
}
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
Log ("Done... Position = %4d\n", fingerPosition);
/* ms_spin (1000);
ledOn ();
for (wrist = SPEED_OFF; wrist < 255; wrist++) {
SetMotorSpeed (wrist, SPEED_OFF);
ms_spin (25);}
ms_spin (1000);
ledOff ();
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
ms_spin (1000);
ledOn ();
for (wrist = SPEED_OFF; wrist > 0; wrist--) {
SetMotorSpeed (wrist, SPEED_OFF);
ms_spin (25);}
ms_spin (1000);
ledOff ();
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
*/
return 0;
}
void WinchControl::Disable()
{
m_speed = 0.0f;
SetMotorSpeed();
}
void BallControl::Disable()
{
m_speed = 0.0f;
SetMotorSpeed();
}
void Shoot(uint16 diff)
{
switch (shooterState)
{
case ssAim: // controls if robot is to shoot or to move back
StartShooterControl();
if (beginBackIR == -1)
beginBackIR = GetRearAveragedSum();
// try to load more balls
if (!IsLoaded())
{
StopMoving();
GuardUp();
if (UpdateTimer(diff, loadTimer, cLoadTimer)) // ran out of time to load new ball - have no balls
shooterState = ssAlignBack;
sDebugPhase = "Loading";
}
else
{
loadTimer = cLoadTimer;
if (GetTargAveragedSum() < cTargOPMinThresh) //need to coarse adj
{
if (doMidAdjust)
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
/*
if (PID::OPDriveAdjustCompleted(sCalibratedBackIRVal * cIRFalloffCenter))
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
}
PID::OPDriveUpdate(diff, sCalibratedBackIRVal * cIRFalloffCenter);
*/
}
else
{
if (aimState == asCW)
{
//RotateCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 + cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 - cAimSpeed);
}
else if (aimState == asCCW)
{
//RotateCCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 - cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 + cAimSpeed);
}
}
if (coarseMaxThreshPassed) //coarse adj control
{
if (GetRearAveragedSum() < beginBackIR * (aimState == asCW ? cTurnIRFalloffCW : cTurnIRFalloffCCW))
{
//switch
aimState = (aimState == asCW ? asCCW : asCW);
coarseMaxThreshPassed = false;
midAjustComplete = false;
doMidAdjust = false;
}
sDebugPhase = "Coarse 2";
}
else
{
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
{
if (!midAjustComplete)
doMidAdjust = true;
coarseMaxThreshPassed = true;
}
sDebugPhase = "Coarse 1";
}
}
else // fine
{
midAjustComplete = false;
doMidAdjust = false;
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
coarseMaxThreshPassed = true;
if (PID::TargOPAdjustCompleted())
{
StopMoving();
shooterState = ssFire;
//sAimTmr = cMaxAimDurationTimer;
}
PID::OPUpdate(true, diff, PID::OPPGainFront, 0, 0);
if (IsLoaded())
if (UpdateTimer(diff, sAimTmr, cMaxAimDurationTimer))
{
StopMoving();
shooterState = ssFire;
}
sDebugPhase = "Fine Adj";
}
}
break;
case ssFire:
StopMoving();
if (shooterReady)
GuardDown();
if (!IsLoaded())
if (UpdateTimer(diff, sShootTmr, cShootTimer))
{
sAimTmr = cMaxAimDurationTimer;
shooterState = ssAim;
}
sDebugPhase = "Fire";
break;
case ssAlignBack:
StopShooterControl();
if (PID::RearOPAdjustCompleted())
{
StopMoving();
shooterState = ssAim;
robotState = rsNavigateBack;
beginBackIR = -1;
sDebugPhase = "******************";
}
PID::OPUpdate(false, diff, PID::OPPGainBack, 0, 0);
sDebugPhase = "Align Back";
break;
}
if (UpdateTimer(diff, LCDTimer, LCD_UPDATE_TIMER))
{
LCD_PRINTNEW("S:");
LCD.print(sDebugPhase);
LCD.setCursor(0, 1);
switch (shooterState)
{
case ssAim:
LCD.print("A ");
LCD.print(GetAveragedLeftTargOPReading());
LCD.print(" ");
LCD.print(GetAveragedRightTargOPReading());
LCD.print(" ");
LCD.print(beginBackIR * cTurnIRFalloffCW);
LCD.print(" ");
LCD.print(GetRearAveragedSum());
break;
case ssFire:
LCD.print("SPD ");
LCD.print(shooterTopFreq);
LCD.print(" ");
LCD.print(shooterBotFreq);
LCD.print(" ");
break;
case ssAlignBack:
LCD.print("B ALGN ");
LCD.print(GetLeftRearOPReading());
LCD.print(" ");
LCD.print(GetRightRearOPReading());
LCD.print(" ");
break;
}
}
if (stopbutton())
{
StopShooterControl();
shooterTopFreq = SetConstant("Shooter Top Freq", cShooterTopFreq, 1/2.0f);
shooterBotFreq = SetConstant("Shooter Bot Freq", cShooterBotFreq, 1/2.0f);
SetConstant("Right OP Gain", cFrontRightOPGain, 1/20.0f);
StartShooterControl();
}
}
