int main(void)
{
SPI_MasterInit();
btInit();
interruptINT1_init();
sei();
btTransmit(0);
//moveRobot(ACTIVATE_HIT);
while(1)
{
BT_SensorValues();
getSensorValues();
setVariables();
dataValues[LIFE] = lifeCount;
//Start of AI program that should keep the robot within the boundaries of the tape track
moveRobot(LED | lifeCount);
if(!lifeCount) {
sensorControlDead();
if(TapeFlag >= 0x03 && counting == 0) {
counting = 1;
timerValue = TIMER_1A_SECOND;
timer_init();
} else if(counting == 2) {
while(1) {
moveRobot(STOP);
}
}
} else {
if(hit == 0) {
activateHitFlag = 0;
hitFlag = 1;
}
if (hit == 1 && activateHitFlag)
{
moveRobot(IR_ON);
moveRobot(ACTIVATE_HIT);
}
else if(hit == 1 && hitFlag == 1) {
hitFlag = 0;
lifeCount = lifeCount >> 1;
timer0_init();
}
else if (hit == 1 && !activateHitFlag) {
moveRobot(IR_OFF);
}
if(!sprayPray) {
idle();
}
}
void TurnLeft()
{
char rightEncoderCount = 0;
char leftEncoderCount = 0;
xQueueReset(MsgQueue_MapEncoder_Interrupt);
moveRobot(-600,600);
while(leftEncoderCount < 56)
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
moveRobot(0,0);
}
void TurnAround()
{
char rightEncoderCount = 0;
char leftEncoderCount = 0;
xQueueReset(MsgQueue_MapEncoder_Interrupt);
moveRobot(600,-600);
while(rightEncoderCount < 112)
{
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
}
moveRobot(0,0);
}
void turnLeft1()
{
int i;
// stop robot before turning
moveRobot(0, 0);
for (i = 0;(i < ROTATIONS_PER_90_DEGREES);i++)
{
nMotorEncoder[RightDrive] = 0; // set encoder values to 0
nMotorEncoder[LeftDrive] = 0;
moveRobot(-100, 100); // turn robot
while ((nMotorEncoder[LeftDrive] < ROTATION_TICKS_1) && (nMotorEncoder[RightDrive] < ROTATION_TICKS_1))
{
}
}
}
bool IDT_CheckForIntersection(char c)
{
bool intersection = ( c == 47 || c == 15 || c == 62 || c == 60 || c == 63 ); //L, R, both
if(intersection)
moveRobot(0, 0);
return intersection;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "wall_following");
ros::NodeHandle n;
g_WallDetection = n.subscribe("/ir_obstacle_detection/IRObstacleSignal", 1, receiveObstacleData); // message from ir sensors
g_KeyboardSub = n.subscribe("/keyboard_control/KeyboardStates", 1, receiveKeyboardStates);
g_pwmPub = n.advertise<differential_drive::PWM>("/motion/PWM", 1);
ros::Rate loop_rate(100);
// check turning direction from the beginning
g_bCheckTurningDirection = true;
while (ros::ok())
{
moveRobot();
loop_rate.sleep();
ros::spinOnce();
}
return EXIT_SUCCESS;
}
void robotField::keyPressEvent(QKeyEvent *event)
{
if(event->key()==Qt::Key_Up)
moveRobot(-5);
if(event->key()==Qt::Key_Down)
moveRobot(5);
if(event->key()==Qt::Key_Right)
rotateRobot(10);;
if(event->key()==Qt::Key_Left)
rotateRobot(-10);
if(event->key()==Qt::Key_0)
rotateRobot(15);
}
int GLWidget::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QGLWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: clicked(); break;
case 1: selectedRobot(); break;
case 2: closeSignal((*reinterpret_cast< bool(*)>(_a[1]))); break;
case 3: toggleFullScreen((*reinterpret_cast< bool(*)>(_a[1]))); break;
case 4: robotTurnedOnOff((*reinterpret_cast< int(*)>(_a[1])),(*reinterpret_cast< bool(*)>(_a[2]))); break;
case 5: moveRobot(); break;
case 6: resetRobot(); break;
case 7: selectRobot(); break;
case 8: moveCurrentRobot(); break;
case 9: resetCurrentRobot(); break;
case 10: moveBall(); break;
case 11: changeCameraMode(); break;
case 12: yellowRobotsMenuTriggered((*reinterpret_cast< QAction*(*)>(_a[1]))); break;
case 13: blueRobotsMenuTriggered((*reinterpret_cast< QAction*(*)>(_a[1]))); break;
case 14: switchRobotOnOff(); break;
case 15: moveRobotHere(); break;
case 16: moveBallHere(); break;
case 17: lockCameraToRobot(); break;
case 18: lockCameraToBall(); break;
default: ;
}
_id -= 19;
}
return _id;
}
/*
* Function:
* float pidMoveToHeading(float heading, uint8_t speed, RobotGlobalStructure *sys)
*
* Will rotate and then move the robot along the given heading at the given speed.
*
* Inputs:
* float heading:
* The heading in degrees that we wish the robot to face (-180 < heading < 180)
* uint8_t speed:
* Absolute speed as a percentage of maximum speed (0-100)
* RobotGlobalStructure *sys:
* A pointer to the sys->pos. structure so we can get IMU.yaw
*
* Returns:
* Will return 0 if the robot moving along the desired heading, otherwise will return the signed
* error
*
* Implementation:
* Works similarly to pidRotateToHeading() except that robot will move along heading at the speed
* specified in the parameters. Direction of travel is controlled by closed loop system with the IMU.
* pErr stores the proportional error value. It is declared as static as they need to retain their
* values between function calls. motorSpeed stores the duty cycle (%) that will be sent to the
* moveRobot() function. First, the function checks that heading is within the required range
* (between -180 and 180 degrees). If it is out of range, the number is scaled down by nfWrapAngle.
* Next, the proportional (or signed) error value is calculated. It is simply the difference between
* the desired heading and the current actual heading. The resulting error value is multiplied by a
* tuning constant and summed. The result of this is the corrective speed and direction to be applied
* to the motors. The absolute value of this is stored in motorSpeed and is then checked to make sure
* that its value is no greater than 100 (the maximum speed of the motors). After that, the
* proportional error is checked to see if it is with 0.5 degrees of the desired heading. If it is,
* and delta yaw is less than 0.5dps, then this is deemed close enough to end seeking the desired
* heading. The static error variable is cleared, the robot is stopped and the function exits with a
* 0 value. If the prior conditions are not met, then the motorSpeed value is passed to the
* moveRobot function in order to correct the error.
*
*/
float pidMoveToHeading(float heading, uint8_t speed, RobotGlobalStructure *sys)
{
static float pErr; //Proportional (signed) error
int32_t rotationSpeed = 0; //Stores turn ratio calculated by PID sum
//Make sure heading is in range (-180 to 180)
heading = nfWrapAngle(heading);
//Make sure speed is in range
speed = capToRangeUint(speed, 0, 100);
//Calculate proportional error values
pErr = heading - sys->pos.facing; //Signed Error
//Force the P controller to always take the shortest path to the destination.
//For example if the robot was currently facing at -120 degrees and the target was 130 degrees,
//instead of going right around from -120 to 130, it will go to -180 and down to 130.
if(pErr > 180)
pErr -= 360;
if(pErr < -180)
pErr += 360;
//If motorSpeed ends up being out of range, then dial it back
rotationSpeed = MTH_KP*pErr;
rotationSpeed = capToRangeInt(rotationSpeed, -100, 100);
moveRobot(0, speed, rotationSpeed);
//If error is less than 0.5 deg and delta yaw is less than 0.5 degrees per second then we can
//return 0 (ie robot is more or less on correct heading)
if((abs(pErr) < MF_FACING_ERR) && (abs(sys->pos.IMU.gyroZ) < MF_DELTA_GYRO_ERR))
return 0;
else
return pErr; //If not, return pErr
}
void chgOrient(PlayerClient *robotc, LaserProxy &lp, Position2dProxy &pp, robot_pos *point, robot_pos *robot){
unsigned char orientn, directn;
double newspeed, newturnrate;
int iters=0;
if ((robot->yaw >= 0) && (robot->yaw <= dtor(90))) orientn=1;
else if ((robot->yaw > dtor(90)) && (robot->yaw <= dtor(180))) orientn=2;
else if ((robot->yaw < 0) && (robot->yaw >= -dtor(90))) orientn=3;
else orientn=4;
if ((robot->x >= point->x) && (robot->y >= point->y)) directn=1;
else if ((robot->x <= point->x) && (robot->y >= point->y)) directn=2;
else if ((robot->x >= point->x) && (robot->y <= point->y)) directn=3;
else directn=4;
newspeed = 0;
iters = MOVE_ITER;
if((directn==1) && (orientn==1)) newturnrate = dtor((180)/TURNFACTOR);
else if ((directn==2) && (orientn==2)) newturnrate = dtor((180)/TURNFACTOR);
else if ((directn==3) && (orientn==3)) newturnrate = dtor((180)/TURNFACTOR);
else if ((directn==4) && (orientn==4)) newturnrate = dtor((180)/TURNFACTOR);
else iters = 0;
for(int i=0; i<iters; i++){
robotc->Read();
moveRobot(lp, pp, &newspeed, &newturnrate);
}
}
task main()
{
waitForStart();
initializeRobot();
wait1Msec(12000);
//sets seeker value
tHTIRS2DSPMode _mode = DSP_1200;
//Starts the first basket movements
moveRobot(firstBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
int seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to second basket movements
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to third basket movements
moveRobot(thirdBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches- 1, forwardSpeedMove, backwards);
}
//No sensor found so score in fourth basket
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
/*
* Function:
* float pidRotateToHeading(float heading, int8_t maxSpeed, RobotGlobalStructure *sys)
*
* Will rotate the robot to face the given heading
*
* Inputs:
* float heading:
* The heading in degrees that we wish the robot to face (-180 < heading < 180)
* RobotGlobalStructure *sys:
* A pointer to the sys->pos. structure so we can get IMU.yaw
*
* Returns:
* Will return 0 if the robot has settled at the desired heading, otherwise will return the signed
* error
*
* Implementation:
* pErr stores the proportional error value. It is declared as static as they need to retain their
* values between function calls. motorSpeed stores the duty cycle (%) that will be sent to the
* moveRobot() function. First, the function checks that heading is within the required range
* (between -180 and 180 degrees). If it is out of range, the number is scaled down by nfWrapAngle.
* Next, the proportional (or signed) error value is calculated. It is simply the difference between
* the desired heading and the current actual heading. The resulting error value is multiplied by a
* tuning constant and summed. The result of this is the corrective speed and direction to be applied
* to the motors. The absolute value of this is stored in motorSpeed and is then checked to make sure
* that its value is no greater than 100 (the maximum speed of the motors). After that, the
* proportional error is checked to see if it is with 0.5 degrees of the desired heading. If it is,
* and delta yaw is less than 0.5dps, then this is deemed close enough to end seeking the desired
* heading. The static error variable is cleared, the robot is stopped and the function exits with a
* 0 value. If the prior conditions are not met, then the motorSpeed value is passed to the
* moveRobot function in order to correct the error.
*
* Improvements:
* the PID controller functionality might be able to be moved to its own function to be used by
* more than one motion function. Haven't implemented yet because not sure if this would create
* problems later when there is the potential for to PID controllers to be running at once. If using
* static vars between calls they could crosstalk.
*
*/
float pidRotateToHeading(float heading, float speed, RobotGlobalStructure *sys)
{
float pErr; //Proportional (signed) angle error
float psErr;
static float isErr; //Proportional Speed Error
static float psErrOld = 0; //Proportional Speed Error
static float iErr = 0; //Integral Error
int32_t motorSpeed; //Stores motorSpeed calculated by PID sum
float maxSpeed = 0;
//Make sure heading is in range (-180 to 180)
heading = nfWrapAngle(heading);
//Make sure max speed is in range (0-180dps)
speed = capToRangeInt(speed, 0, 180);
//Calculate proportional error values
pErr = heading - sys->pos.facing; //Signed Error
psErr = (speed - abs(sys->pos.IMU.gyroZ));
isErr += psErr;
//Calculate integral error
if(abs(pErr) < 4)
iErr += pErr;
else
iErr = 0;
//Force the P controller to always take the shortest path to the destination.
//For example if the robot was currently facing at -120 degrees and the target was 130 degrees,
//instead of going right around from -120 to 130, it will go to -180 and down to 130.
if(pErr > 180)
pErr -= 360;
if(pErr < -180)
pErr += 360;
//If motorSpeed ends up being out of range, then dial it back
motorSpeed = RTH_KP*pErr + RTH_KI*iErr;
maxSpeed = RTH_KPS*speed + RTH_KIS*isErr + RTH_KDS*(psErrOld - psErr);
maxSpeed = capToRangeFlt(maxSpeed, 10, 100);
//maxSpeed = speed;
motorSpeed = capToRangeInt(motorSpeed, (int)-maxSpeed, (int)maxSpeed);
psErrOld = psErr;
//If error is less than 0.5 deg and delta yaw is less than 0.5 degrees per second then we can
//stop
if((abs(pErr) < MF_FACING_ERR) && (abs(sys->pos.IMU.gyroZ) < MF_DELTA_GYRO_ERR))
{
stopRobot(sys);
iErr = 0; //Clear the static vars so they don't interfere next time we call this
isErr = 0; //function
return 0;
} else {
moveRobot(0, motorSpeed, 100);
return pErr; //If not, return pErr
}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
if(htim->Instance == TIM5){
moveRobot();
}
else if (htim->Instance == TIM7){
// if(Robot.sendOK)
SendData();
}
}
bool_t Battle::runBattle()
{
bool_t rv = true;
uint16_t dmg;
uint16_t range = getRange(Robot1->getLocation(), Robot2->getLocation());
if (turn == 0)
{
moveRobot(Robot1, 20, range);
dmg = Robot2->Defend(Robot1->Attack(range));
cout << "Robot1: "<< Robot::robotNames[Robot1->getType()] << " attacks "
<< "Robot2: "<< Robot::robotNames[Robot2->getType()] << " for " << dmg <<" damage. "
<< "Robot2's health is now " << Robot2->getHealth() << endl;
turn = 1;
}
else
{
moveRobot(Robot2, -20, range);
dmg = Robot1->Defend(Robot2->Attack(range));
cout << "Robot2: "<< Robot::robotNames[Robot2->getType()] << " attacks "
<< "Robot1: "<< Robot::robotNames[Robot1->getType()] << " for " << dmg <<" damage. "
<< "Robot1's health is now " << Robot1->getHealth() << endl;
turn = 0;
}
if((Robot1->getHealth() > 0) && (Robot2->getHealth() > 0)) // battle isn't finished
{
rv = true;
}
else // battle is complete
{
if (Robot1 -> getHealth() == 0)
{
cout << "Robot2: "<< Robot::robotNames[Robot2->getType()] << " wins!" <<endl;
}
else
{
cout << "Robot1: "<< Robot::robotNames[Robot1->getType()] << " wins!" <<endl;
}
rv = false;
}
return rv;
}
void moveRobotOneRouteCoordinate(int power)
{
int differential; // Holds the encoder tick differential between right and left.
int correction; // Holds a calculated correction value.
int correctedLeftPower;
int correctedRightPower;
// Reset the encoder tick counts.
nMotorEncoder[RightDrive] = 0;
nMotorEncoder[LeftDrive] = 0;
while (nMotorEncoder[LeftDrive] < TICKS_PER_INCH && nMotorEncoder[RightDrive] < TICKS_PER_INCH)
{
differential = (nMotorEncoder[RightDrive] - nMotorEncoder[LeftDrive]); // Find the different between the left and right encoder.
correction = (int) (differential * 0.1); // The correction value is a 10th of the differential.
// Keep correction value within -10 and 10 (i.e. not more than a 10th of the power plus/minus).
if (correction < -10)
{
correction = -10;
}
else if (correction > 10)
{
correction = 10;
}
correctedLeftPower = (power + correction); // Correct the left power value.
// Keep the corrected left power value within -100 and 100
if (correctedLeftPower < -100)
{
correctedLeftPower = -100;
}
else if (correctedLeftPower > 100)
{
correctedLeftPower = 100;
}
correctedRightPower = (power - correction); // Correct the right power value.
// Keep corrected right power value within -100 and 100
if (correctedRightPower < -100)
{
correctedRightPower = -100;
}
else if (correctedRightPower > 100)
{
correctedRightPower = 100;
}
// Drive the robot with the corrected power values.
moveRobot(correctedLeftPower, correctedRightPower);
wait1Msec(10);
}
}
void MoveDistance(int d, int speed) // 1/17th of an inch
{
xQueueReset(MsgQueue_MapEncoder_Interrupt);
int rightEncoderCount = 0;
int leftEncoderCount = 0;
if(d < 0)
{
moveRobot(-1*speed, -1*speed);
d = -1*d;
}
else
{
moveRobot(speed, speed);
}
while( rightEncoderCount < d && leftEncoderCount < d )
{
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
}
moveRobot(0,0);
}
task main()
{
waitForStart();
initializeRobot();
wait1Msec(12000);
//sets seeker value
tHTIRS2DSPMode _mode = DSP_1200;
//Starts the first basket movements
moveRobot(firstBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to second basket movements
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to third basket movements
moveRobot(thirdBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches- 1, forwardSpeedMove, backwards);
}
//No sensor found so score in fourth basket
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
/*!
@brief Thread that controls movement of arm and magnet state.
@param argument Unused
@retval None.
*/
void armThread (void const *argument) {
uint8_t data[RECEIVE_DATA_SIZE];
parkRobot(&robot);
osDelay(1000);
// uint32_t prevY1=0, prevAngle1=0, nextY1=rand()%7, nextAngle1=rand()%14;
// uint32_t prevY2=0, prevAngle2=0, nextY2=rand()%7, nextAngle2=rand()%14;
while(1) {
wireless_RX(&receiver);
osMutexWait(receiver.mutexID, osWaitForever);
for (uint32_t i=0; i<sizeof(data)/sizeof(data[0]); i++) {
data[i] = receiver.data[i];
}
osMutexRelease(receiver.mutexID);
moveRobot(&robot, data[0], data[1], data[2]);
if (data[3]) {
turnMagnetOn(&magnet);
} else {
turnMagnetOff(&magnet);
}
// moveRobot(&robot, prevY1, 0, prevAngle1);
// turnMagnetOn(&magnet);
// osDelay(500);
// moveRobot(&robot, nextY1, 0, nextAngle1);
// turnMagnetOff(&magnet);
// osDelay(500);
//
// moveRobot(&robot, prevY2, 0, prevAngle2);
// turnMagnetOn(&magnet);
// osDelay(500);
// moveRobot(&robot, nextY2, 0, nextAngle2);
// turnMagnetOff(&magnet);
// osDelay(500);
//
// prevY1=nextY1;
// prevAngle1=nextAngle1;
// prevY2=nextY2;
// prevAngle2=nextAngle2;
//
// nextY1=rand()%7;
// nextAngle1=rand()%14;
// nextY2=rand()%7;
// nextAngle2=rand()%14;
osDelay(100);
}
}
int robotField::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QGraphicsView::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: moveRobot((*reinterpret_cast< int(*)>(_a[1]))); break;
case 1: rotateRobot((*reinterpret_cast< int(*)>(_a[1]))); break;
default: ;
}
_id -= 2;
}
return _id;
}
/*
* main.c
*/
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initializeMotor();
initializeADC10();
P1DIR |= BIT0;
P1DIR |= BIT6;
for (;;){
leftSensorVoltage=getLeftSensorVoltage();
if(leftSensorVoltage < LEFTDISTANCE){
P1OUT &= ~BIT0;
moveRobot(SMALLRIGHT);
}
else{
P1OUT |= BIT0;
moveRobot(FORWARD);
}
rightSensorVoltage=getRightSensorVoltage();
if(rightSensorVoltage<RIGHTDISTANCE){
P1OUT &= ~BIT6;
moveRobot(SMALLLEFT);
}
else{
P1OUT |= BIT6;
moveRobot(FORWARD);
}
}
return 0;
}
/*
* Function:
* char pidAdvancedMove(float heading, float facing, uint8_t speed,
* uint8_t maxTurnRatio, RobotGlobalStructure *sys)
*
* Will move the robot along a heading, and also rotate the robot to face the given facing,
* using closed loop control from the mouse and IMU to achieve it.
*
* Inputs:
* float heading:
* The absolute (arena) heading that the robot should travel along
* float facing:
* The absolute (arena) facing that the robot should face
* uint8_t speed:
* The maximum motor speed (0-100%)
* uint8_t maxTurnRatio:
* The percentage of rotational speed to be applied to the motors (ie how fast the robot will
* rotate towards the desired facing). 0% means the robot will not rotate at all. %100 means that
* The robot will only rotate on the spot until the desired facing is achieve before setting off
* on the desired heading. Anything in between will have the robot gradually rotate while
* travelling along the desired heading.
* RobotGlobalStructure *sys:
* Pointer to the global robot data structure
*
* Returns:
* 0 when the robot has achieved the desired facing, otherwise the proportional error between the
* desired facing and the current facing of the robot.
*
* Implementation:
* See pidMoveToHeading() above as this function is based on that. The difference is that this
* two control mechanisms, one from the IMU and one from the optical sensor for the facing control
* and heading control respectively.
*
* Improvements:
* [Ideas for improvements that are yet to be made](optional)
*
*/
float pidAdvancedMove(float heading, float facing, uint8_t speed,
uint8_t maxTurnRatio, RobotGlobalStructure *sys)
{
//float pErrH; //Proportional (signed) heading error
float pErrF; //Proportional (signed) facing error
float facingCorrection = 0; //Stores turn ratio calculated by PID sum
//float headingCorrection = 0; //Stores heading correction calculated by PID sum
//Make sure heading and facing is in range (-180 to 180)
heading = nfWrapAngle(heading);
facing = nfWrapAngle(facing);
//Make sure speed and maxTurnRatio is in range
speed = capToRangeUint(speed, 0, 100);
maxTurnRatio = capToRangeUint(maxTurnRatio, 0, 100);
//Calculate proportional error values
//pErrH = heading - sys->pos.heading; //Signed Error (heading)
pErrF = facing - sys->pos.facing; //Signed Error (facing)
//Force the P controller to always take the shortest path to the destination.
//For example if the robot was currently facing at -120 degrees and the target was 130 degrees,
//instead of going right around from -120 to 130, it will go to -180 and down to 130.
if(pErrF > 180)
pErrF -= 360;
if(pErrF < -180)
pErrF += 360;
//if(pErrH > 180 || pErrH < -180)
// pErrH *= -1;
//Calculate the correction figures
//headingCorrection = AMH_KP*pErrH;
facingCorrection = AMF_KP*pErrF;
//If the correction values end up being out of range, then dial them back
facingCorrection = capToRangeFlt(facingCorrection, -maxTurnRatio, maxTurnRatio);
//headingCorrection = capToRangeFlt(headingCorrection, -180, 180);
//Get the robot moving to correct the errors.
moveRobot((heading - sys->pos.facing) , speed, facingCorrection);
//If error is less than 0.5 deg and delta yaw is less than 0.5 degrees per second then we can
//return 0 (ie robot is more or less on correct heading)
if((abs(sys->pos.IMU.gyroZ) < MF_DELTA_GYRO_ERR) && (abs(pErrF) < MF_FACING_ERR))
return 0;
else
return pErrF; //If not, return pErrF
}
task main()
{
initializeRobot();
//waitForStart();
moveDownWrist();
moveRobot();
liftArm();
wait1Msec(500);
rightTurnRobot();
wait1Msec(750);
shortMoveRobot();
wait1Msec(500);
moveUpWrist();
lowerArm();
}
task main()
{
initializeRobot();
// waitForStart();
long i;
bool lookingForBeacon = true;
for (i = 0;(i < 20);i++)
{
moveRobotOneRouteCoordinate(100);
if (isBeaconInRange(irSensor) == true)
{
int sector;
if (i < 33)
{
sector = 5;
}
else
{
sector = 6;
}
if (SensorValue[irSensor] == sector) // robot has reached the desired sector
{
if (sector == 6)
{
wait1Msec(100);
}
DispenseBlock();
lookingForBeacon = false;
}
}
}
turnLeft1();
wait1Msec(10);
for (i = 0;(i < 35);i++)
{
moveRobotOneRouteCoordinate(100);
}
turnLeft2();
for (i = 0;(i < 50);i++)
{
moveRobotOneRouteCoordinate(100);
}
moveRobot(0, 0);
}
void RobotManager::Run()
{
isRunning = true;
WayPoint* nextWayPoint;
nextWayPoint = wpm.getNextWayPoint();
cout<<"next waypoint: "<<nextWayPoint->x_Coordinate<<","<<nextWayPoint->y_Coordinate<<","<<nextWayPoint->yaw<<endl;
//move to start position
moveToStartPoint(nextWayPoint);
while(wpm.haveMoreWayPoints())
{
robot->Refresh();
cout<<"current position: ["<<robot->GetX()<<","<<robot->GetY()<<","<<robot->GetYaw()<<"]"<<endl;
nextWayPoint = wpm.getNextWayPoint();
moveRobot(nextWayPoint);
}
cout<<"100% completed"<<endl;
}
//The main task will run the robot initialization and then will wait for the signal from FCS
//to begin and enter the continuous loop that recieves joystick values and executes functions
//with this data.
task main()
{
//initializeRobot(); //Sets servos into
waitForStart(); // Waits for start of tele-op phase
int sloMoCounter = 0;
int directionCounter = 0;
while ( true )
{
//Updates joystick variables with data from joystick station
getJoystickSettings( joystick );
moveRobot();
sloMoCounter = toggleSloMo(sloMoCounter);
directionCounter = toggleDirection(directionCounter);
//runConveyer();
//moveLift();
//moveServo();
}
}
// calculate robot's heading from compass and how far it has moved since the last time this was called, update current grid location
// by calculating how many grid positions it have moved (and in what direction).
// this can and should be called frequently from the main task loop.
void getCurrentPosition()
{
int CurrentDistanceTraveled;
int CurrentDirection = 0;
//CurrentDistanceTraveled = (SensorValue[RightDrive] / 720) * WHEEL_DIAMETER;
DistanceTraveled += CurrentDistanceTraveled;
//if (DistanceTraveled > 0) {
// moveRobot(0, 0);
// StopAllTasks();
//}
//CurrentDirection = SensorValue[Compass];
// reset rotation tick counter since we've already read its value.
SensorValue[RightDrive] = 0;
// now, use distance travelled and heading to calculate where we are on the field.
if (DistanceTraveled >= 5)
{
moveRobot(0, 0);
StopAllTasks();
//CurrentRouteXIndex++;
//CurrentRouteYIndex++;
//DistanceTraveled = 0;
//if (Near(CurrentDirection, InitialDirection))
//{
// CurrentYCoord += CurrentDistanceTraveled;
//}
//else if (Near(CurrentDirection, ((InitialDirection + 90) % 360)))
//{
// CurrentXCoord += CurrentDistanceTraveled;
//}
//else if (Near(CurrentDirection, ((InitialDirection + 180) % 360)))
//{
// CurrentYCoord -= CurrentDistanceTraveled;
//}
//else if (Near(CurrentDirection, ((InitialDirection + 270) % 360)))
//{
// CurrentYCoord -= CurrentDistanceTraveled;
//}
}
}
void ExecuteTurn(char data)
{
PLIB_PORTS_Write(PORTS_ID_0, PORT_CHANNEL_B, data);
switch(data)
{
case 'r':
TurnRight();
break;
case 'l':
TurnLeft();
break;
case 'f':
MoveDistance(16, 600);
break;
case 't':
TurnAround();
MoveDistance(16, 600);
break;
case 'S':
moveRobot(0, 0);
while(true);
}
}
//Controls the different sensor values and calls functions accordingly
void idle(){
moveRobot(DEACTIVATE_LASER);
if (distanceValue <= 20)
{
leftOrRight = 1;
backing = 1;
backnTurn = 1;
timerValue = TIMER_1A_SECOND/2;
timer_init();
}
else if(frontLeftTape != 0){
leftOrRight = 1;
backing = 1;
backnTurn = 1;
timerValue = TIMER_1A_SECOND/2;
timer_init();
}
else if(frontRightTape != 0){
leftOrRight = 0;
backing = 1;
backnTurn = 1;
timerValue = TIMER_1A_SECOND/2;
timer_init();
}
else if (IRFront != 2 && IRFront < 8){
if(!laserCd){
moveRobot(ACTIVATE_LASER);
sprayPray = 1;
turning = 1;
timerValue = TIMER_1A_SECOND/4;
timer_init();
}
else{
forward = 1;
}
}
else if (!IRFound){
if(IRLeft != 2 && IRLeft < 8){
//moveRobot(ACTIVATE_LASER);
leftOrRight = 0;
turning = 1;
IRFound = 1;
timerValue = TIMER_1A_SECOND*0.8;
timer_init();
//turn(TIMER_1A_SECOND);
}
else if(IRRight != 2 && IRRight < 8){
//moveRobot(ACTIVATE_LASER);
leftOrRight = 1;
turning = 1;
IRFound = 1;
timerValue = TIMER_1A_SECOND*0.8;
timer_init();
//turn(TIMER_1A_SECOND);
}
}
/*else if ((frontTapeValues == 0x00)){
moveRobot(MOVE_FORWARD_FAST);
}*/
}
void IDT_CorrectDirection(char c)
{
switch(c)
{
//NORMAL
case 12: //00110011
{
moveRobot(600,600);
break;
}
//WOBBLES
//Off left
case 8: //00111001
{
moveRobot(400,0);
break;
}
case 40: //00111100
{
moveRobot(400,0);
break;
}
case 32: //00111001
{
moveRobot(400,0);
break;
}
case 48: //00111100
{
moveRobot(400,0);
break;
}
case 16: //00111001
{
moveRobot(400,0);
break;
}
//Off right
case 2: //00100111
{
moveRobot(0,400);
break;
}
case 6: //00001111
{
moveRobot(0,400);
break;
}
case 4: //00001111
{
moveRobot(0,400);
break;
}
case 3: //00001111
{
moveRobot(0,400);
break;
}
case 1: //00001111
{
moveRobot(0,400);
break;
}
xQueueReset(MsgQueue_MapSensor_Interrupt);
}
}
int main(void)
{
int pwm;
int adc_on;
int adc_off;
uint16_t i;
element elem;
robot.rotation_target = 0;
robot.right_motor_pos =0;
robot.left_motor_pos =0;
robot.right_motor_target =0;
robot.left_motor_target =0;
SysTick_Config(168000000/8/1000000); // interrupr 1000000 per secound
initTimer3(1);
initMotors();
initEncoders();
initUsart3();
// enableUSART3RXNEInterrupt();
initADC();
initIRSensors();
initLED();
initSPI2();
LED_OFF;
waitMs(1000);
LED_ON;
initPIDStructures();
stopMotors();
resetEncoders();
updateRobotState();
robot.left_ir_sensor_target = measures.left_ir_sensor;
robot.right_ir_sensor_target = measures.right_ir_sensor;
initQueue(&robot_queue);
//addMove(&robot_queue,FORWARD,2000);
addMove(&robot_queue,ROTATE,20000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,0);
// addMove(&robot_queue,ROTATE,6000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,6000);
// addMove(&robot_queue,ROTATE,12000);
// addMove(&robot_queue,FORWARD,9000);
// addMove(&robot_queue,ROTATE,12000);
// addMove(&robot_queue,ROTATE,3000);
// nextMove();
LED_ON;
while(1)
{
if ( isBatteryWeak() ){
stopMotors();
blinkLed();
}
if (flags.update_robot == 1){
moveRobot();
updateRobotState();
flags.update_robot =0;
}
if (flags.usart_custom == 1){
USART3_transmitString(itoa((int) robot.rotation, buf,10));
USART3_transmitString(" ");
USART3_transmitString(itoa((int) robot.rotation_target, buf,10));
USART3_transmitString("\n");
}
}
// flags.usart_custom == 0;
// USART3_transmitString("lewe_kolo_diff ");
// USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10));
// USART3_transmitByte('\n');
// USART3_transmitString("lewe kolo pwm ");
// USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
//
// USART3_transmitString("prawe kolo diff ");
// USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10));
// USART3_transmitByte('\n');
// USART3_transmitString("prawe kolo pwm ");
// USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
// }
//
// if (flags.usart_graph == 1){
// static uint8_t usart_start = 1;
// if (usart_start ){
// usart_start=0;
// USART3_transmitString("\n");
// USART3_transmitString("\n");
// USART3_transmitString("\n");
// USART3_transmitString("__start__\n");
// USART3_transmitString("lewe_kolo_pozycja ");
// USART3_transmitString("lewe_kolo_cel ");
// USART3_transmitString("lewe_kolo_diff ");
// USART3_transmitString("lewe_kolo_pwm_T ");
// USART3_transmitString("prawe_kolo_pozycja ");
// USART3_transmitString("prawe_kolo_cel ");
// USART3_transmitString("prawe_kolo_diff ");
// USART3_transmitString("prawe_kolo_pwm_T \n");
// USART3_transmitString(" \n");
// }
//
// USART3_transmitString(itoa((int) robot.right_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.right_motor_target , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_target , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10));
// USART3_transmitString(" ");
// USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10));
// USART3_transmitByte('\n');
// flags.usart_graph = 0;
// }
// }
//
return 0;
}
