/*
* main.c
*/
void main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initializeSensors();
setUpTimerA();
turnLeftSlight();
for (;;) {
int left = checkLeftSensor(), center = checkCenterSensor(), right =
checkRightSensor();
if (left > 0x240) {
fullStop();
turnRightSlight();
fullStop();
} else if (center > 0x2FF) {
fullStop();
__delay_cycles(10000);
reverse();
__delay_cycles(10000);
fullStop();
__delay_cycles(10000);
turnRightSlight();
__delay_cycles(10000);
turnRightSlight();
__delay_cycles(10000);
fullStop();
} else {
turnLeftSlight();
}
}
}
bool Port::safeMove(geometry::Vector destination , MovingObj& agent, bool& rotating) {
// calculate speed
int sendSpeed = params::safeSPEED;
int dist = (destination - agent.COM).size();
if (dist <= params::DEC_DIST) {
// obey target point rules
if(dist <= params::REACH_DIST) {
fullStop();
return false;
}
sendSpeed = ((params::safeSPEED - params::minSPEED) * (dist-10) / params::DEC_DIST) + params::minSPEED;
}
// else {
// // obey origin point rules
// sendSpeed = ((params::safeSPEED - speedAtPrev) * (distToPrev+10) / params::DEC_DIST) + speedAtPrev;
// }
// calculate angle
geometry::Vector sendV = (destination - agent.COM);
int fixedDirection = agent.direction >= 0 ? agent.direction - 180 : agent.direction + 180;
int angle = sendV.angle() - fixedDirection;
if (angle > 180)
angle -= 360;
if (angle < -180)
angle += 360;
if (abs(angle) > params::rotationPenalty || (rotating && abs(angle) > params::rotationOutPenalty) ) {
cout << "$$$$$$$$$$$" << angle << endl;
sendSpeed = 0;
angle = angle / 3.0;
angle = (abs(angle)/angle) * max(abs(angle), params::minRotation);
angle = (abs(angle)/angle) * min(abs(angle), params::maxRotation);
rotating = true;
}
else rotating = false;
// cout << "COM: " << agent.COM << " -path: " << path[path.size()-1] << " -border: " << (path[path.size()-1] - agent.COM).size()<< endl;
// cout << "DIR: " << fixedDirection << " -angle: " << (path[index] - agent.COM).angle() << " -drive: " << angle << endl;
// cout << "SPEED: " << sendSpeed << endl;
// if (sendSpeed != 0) sendSpeed = 100;
talkToSetare(sendSpeed , 0 , angle*1);
if (rotating) {
usleep(params::rotationDuration);
fullStop();
}
return true;
}
/*
* AUTHOR:NIK TAORMINA CADET USAF
* DATE:19 NOV 13
* This code is meant to move a robot with two DC motors.
*/
void main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
setUpTimerA();
turnRightSlight();
fullStop();
__delay_cycles(1000000);
turnLeftSlight();
fullStop();
__delay_cycles(1000000);
turnRightLarge();
fullStop();
__delay_cycles(1000000);
turnLeftLarge();
fullStop();
__delay_cycles(1000000);
forward();
fullStop();
__delay_cycles(1000000);
reverse();
fullStop();
__delay_cycles(1000000);
}
void TankDrive::doDrive(int driveTime)
{
_mpuRet = mympu_update();
float baseAngle = mympu.ypr[0];
long startTime = millis();
long cTime = 0;
int cSpeed = 150;
float cAngle = 0;
float correction=0;
float correctLeft=0;
float correctRight=0;
_simpleDeadband = 0.75;
_simpleKp=20;
TrapPath.setRunTime(driveTime);
TrapPath.setStartTime(startTime);
//Using Standard P to control direction
while (millis() - startTime < driveTime) {
_mpuRet = mympu_update();
_currentAngle = mympu.ypr[0];
cSpeed = TrapPath.getSetPoint(millis()-startTime);
cAngle = baseAngle-_currentAngle;
if (_usePID) {
correction = MotorPID.Compute(cAngle,baseAngle);
correctLeft = -correction;
correctRight = correction;
}
else { //use standard Kp multiplier
if (cAngle>_simpleDeadband){
correctLeft = -cAngle*_simpleKp;
correctRight = cAngle*_simpleKp;
}
else if (cAngle<(_simpleDeadband*-1)){
correctLeft = cAngle*_simpleKp;
correctRight = -cAngle*_simpleKp;
}
}
//Serial Monitor report of P follwing
Serial.print("Angle: ");Serial.print(cAngle);
Serial.print(" Speed: ");Serial.print(cSpeed);
//Serial.print(" Correct Left: ");Serial.print(correctLeft);
//Serial.print(" Correct Right: ");Serial.println(correctRight);
Serial.print(" Correct Left: ");Serial.print(constrain(cSpeed+correctLeft,5,255));
Serial.print(" Correct Right: ");Serial.println(constrain(cSpeed+correctRight,5,255));
digitalWrite(_speedPinLeft, constrain(cSpeed+correctLeft,0,255));
digitalWrite(_speedPinRight, constrain(cSpeed+correctRight,0,255));
}
fullStop();
}
void TankDrive::doDrive(int driveTime)
{
long startTime = millis();
long cTime = 0;
int cSpeed = 150;
while (millis() - startTime < driveTime) {
digitalWrite(_speedPinLeft, cSpeed);
digitalWrite(_speedPinRight, cSpeed);
}
fullStop();
}
void Blob::die(){
MessageHandler::Instance()->createMessage(2,this,BlobGame::instance(),this,100);
addStatus(Dead);
fullStop();
if(ContainsFlags(m_UnitStatus,Consume) && m_Target){
unsigned int state = m_Target->getStatus();
RemoveFlag(&state,Consumed);
m_Target->setStatus(state);
Enemy* enemy = (Enemy*)m_Target;
enemy->search();
}
//play death animation
}
task main()
{
wait1Msec(2000);
//start going forward
motorsGoTo(motorD, motorE, 30, 500);
while(SensorValue(sonar) > 30 || SensorValue(sonar) == -1){
//do nothing
}
//stop
fullStop(motorD, motorE, 250);
parkingBrake(motorD, motorE, 25400);
}
void cycleArm()
{
servo[basketServo]=basketServoUp; //raise bucket
wait1Msec(bucketTime); //wait for bucket to raise
motor[armMotor]=armMotorUp; //raise arm
wait1Msec(armTime); //wait for arm and bucket
motor[armMotor]=0; //stop the arm
fullStop(); //wait for no momentum
servo[basketServo]=basketServoDown; //drop bucket and cube
wait1Msec(bucketTime); //process time
servo[basketServo]=basketServoUp;
wait1Msec(bucketTime);
motor[armMotor]=-armMotorUp; //lower the arm
while(SensorValue[touchSensor]==touchUnpressed){ //wait for the arm to lower
}
motor[armMotor]=0; //stop arm at bottom
}
void setup() {
pinMode( SPDA , OUTPUT );
pinMode( SPDB , OUTPUT );
pinMode( STANDBY , OUTPUT );
pinMode( STATUS_LED , OUTPUT );
pinMode( DIR1A , OUTPUT );
pinMode( DIR2A , OUTPUT );
pinMode( DIR1B , OUTPUT );
pinMode( DIR2B , OUTPUT );
myservo.attach(HAXXXOR); // attaches the servo pin
initservo();
fullStop(); //init's all previous outputs
Serial.begin(USBSERIAL); // init USB Serial (console)
Serial1.begin(BTGPSRATE); // init pin0/1 Serial (bluetooth)
delay(1234);
setupBT(3210);
while(readBT() != CMD_STOP); // safety frist? ¯\(°_o)/¯ I DUNNO LOL
}
void loop() {
Serial1.print('r'); // request data from droid
char bt = readBT();
if(bt == CMD_STOP)
fullStop();
else if(bt == CMD_DRIVE) {
if(status == CMD_STOP) {
status = CMD_DRIVE;
digitalWrite(STANDBY, HIGH);
digitalWrite(STATUS_LED, HIGH); // indicate status
}
x = readBT(); y = readBT(); z = readBT();
int l = z + y, r = z - y;
int L = (l < 0 ? -l : l)<<1;
int R = (r < 0 ? -r : r)<<1;
L = L > 255 ? 255 : L < 0 ? 0 : L;
R = R > 255 ? 255 : R < 0 ? 0 : R;
setMotor(MOTA, -l > 0 ? FORWARD : BACKWARD, L);
setMotor(MOTB, -r > 0 ? FORWARD : BACKWARD, R);
}
}
void TankDrive::doTurn(int turnDegrees)
{
_mpuRet = mympu_update();
float baseAngle = mympu.ypr[0];
int cSpeed = 120;
float cAngle = 0;
float correction=0;
float correctLeft=0;
float correctRight=0;
while(cAngle < turnDegrees) {
_mpuRet = mympu_update();
_currentAngle = mympu.ypr[0];
cAngle = baseAngle-_currentAngle;
cAngle = abs(cAngle);
digitalWrite(_speedPinLeft, cSpeed);
digitalWrite(_speedPinRight, cSpeed);
}
fullStop();
}
void IGV::runProgram ()
{
addVisibleLinesToMap ();
if (autonomousMode)
{
//cout << "current rotation: " << rotation << endl;
// still have goals to reach
if (env.waypoints.size () > 0){
// a route to a goal has been mapped out already
if (followingPath)
{
pathNode = pf.getCurPathNode ();
// check if current node is close enough to move on to next
float dist_to_node = position.distance (pathNode);
if (dist_to_node < pf.getGoalDistance () )
{
// cout << "Reached the next node in the path\n";
if (pf.path.size () > 1)
{
pf.setNextPathNode ();
// cout << "Setting the next path node\n";
// cout << "Nodes remaining: " << pf.path.size () << endl;
//pathNode = pf.getCurPathNode ();
}
// reached the end of current path to a waypoint
else
{
// cout << "Reached the waypoint, going on to the next\n";
fullStop ();
pf.clear ();
env.waypoints.pop_front();
followingPath = false;
}
}
else{
// align to next node in the path
float theta = angleTo (pathNode);
//cout << "angle to next node: " << theta << endl;
// check for a potential collision, if one might occur then the
// current path should be abandoned and a new one should be formed
vector <pair <vertex, Line*> > closeWalls = env.lineMap->getObjectsInRegion (pathNode - pf.getWallDistance (), pathNode + pf.getWallDistance () );
if (closeWalls.size () > 0){
fullStop ();
pf.clear ();
followingPath = false;
return;
}
// set a forward speed that is related to the angle to the next node
// as well as the distance to the next node
if (theta <= 45.0){
setRotateSpeed ( max ( 0.0, sin (theta/DEGREES_PER_RADIAN) ) );
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN) ) );
}
else if (theta >= 315.0){
setRotateSpeed ( max ( 0.0, -sin (theta/DEGREES_PER_RADIAN) ) );
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN) ) );
}
// turn left
else if (theta < 180.0){
setRotateSpeed (2.0);
//setForwardSpeed (0);
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN)/3.0 ) );
}
// turn right
else {
setRotateSpeed (-2.0);
//setForwardSpeed (0);
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN)/3.0 ) );
}
}
}
// generate a path to a goal
else
{
// start a new path
if ( !pf.searching () ){
findPath (env.waypoints.front());
}
// continue expanding on path search until a path is found or the
// specified waypoint is determined to be unreachable
else{
if ( !pf.done () ){
pf.expand ();
if (pf.pathImpossible ()){
pf.clear ();
env.waypoints.pop_front();
//cout << "Mission impossible~\n";
}
}
else{
followingPath = true;
}
}
}
}
}
}
void toBasket()
{
scaleMove(100, 100, 600); //full power forward for 600ms
fullStop();
}
Port::~Port(){
fullStop();
delete port;
}
bool Port::move(vector<geometry::Vector>& path, vector<int>& pathSpeeds, MovingObj& agent, int& index, bool& rotating) {
if(index < 0){
fullStop();
return false;
}
// calculate speed
// bool fullStop = false;
int sendSpeed = params::SPEED;
int distToNext = index >= 0 && index < path.size() ?
(agent.COM - path[index]).size() : std::numeric_limits<int>::max();
int distToPrev = index-1 >= 0 && index-1 < path.size() ?
(agent.COM - path[index-1]).size() : std::numeric_limits<int>::max();
int speedAtPrev = index-1 >= 0 && index-1 < path.size() ? pathSpeeds[index-1] : 0;
if(index == 0 && distToNext <= 50){
fullStop();
return false;
}
if (distToNext <= distToPrev) {
// obey target point rules
if(distToNext <= params::REACH_DIST) {
index--;
fullStop();
// usleep(500*1000);
return move(path, pathSpeeds, agent, index, rotating);
}
if(distToNext <= params::DEC_DIST) {
if (pathSpeeds[index] == -1) {
sendSpeed = params::minSPEED;
fullStop();
}
else sendSpeed = ((params::SPEED - pathSpeeds[index]) * (distToNext-10) / params::DEC_DIST) + pathSpeeds[index];
}
}
else {
// obey origin point rules
if(distToPrev <= params::ACC_DIST) {
sendSpeed = ((params::SPEED - speedAtPrev) * (distToPrev+10) / params::DEC_DIST) + speedAtPrev;
}
}
sendSpeed = max(sendSpeed, params::minSPEED);
sendSpeed = min(sendSpeed, params::maxSPEED);
// calculate angle
geometry::Vector sendV = (path[index] - agent.COM);
int fixedDirection = agent.direction >= 0 ? agent.direction - 180 : agent.direction + 180;
int angle = sendV.angle() - fixedDirection;
if (angle > 180)
angle -= 360;
if (angle < -180)
angle += 360;
int way = params::properDegrees[0];
for(int i = 0; i < params::properDegrees.size(); i++) {
if (abs(angle - params::properDegrees[i]) < abs(angle - way))
way = params::properDegrees[i];
}
if (way == -180) way = 180;
angle -= way;
if (angle > 180)
angle -= 360;
if (angle < -180)
angle += 360;
if (abs(angle) > params::rotationPenalty || (rotating && abs(angle) > params::rotationOutPenalty) ) {
cout << "$$$$$$$$$$$" << angle << endl;
sendSpeed = 0;
angle = angle / 3.0;
angle = (abs(angle)/angle) * max(abs(angle), params::minRotation);
angle = (abs(angle)/angle) * min(abs(angle), params::maxRotation);
rotating = true;
}
else rotating = false;
// cout << "WAY: " << way << endl;
// cout << "COM: " << agent.COM << " -path: " << path[path.size()-1] << " -border: " << (path[path.size()-1] - agent.COM).size()<< endl;
// cout << "DIR: " << fixedDirection << " -angle: " << (path[index] - agent.COM).angle() << " -drive: " << angle << endl;
// cout << "SPEED: " << sendSpeed << endl;
// if (sendSpeed != 0) sendSpeed = 100;
talkToSetare(sendSpeed , way , angle*1);
if (rotating) {
usleep(params::rotationDuration);
fullStop();
}
return true;
}
