void followIR()
{
if (readIR()< 5 && readIR()>0)
{
motor[Motorleft]=-50;
motor[Motorright]=50;
nxtDisplayTextLine(3,"Left");
}
else if (readIR()> 5)
{
motor[Motorleft]=50
;
motor[Motorright]=-50;
nxtDisplayTextLine(3,"Right");
}
else if (readIR()== 5)
{
motor[Motorleft]=25;
motor[Motorright]=25;
nxtDisplayTextLine(3,"Forward");
}
else
{
motor[Motorleft]=0;
motor[Motorright]=0;
nxtDisplayTextLine(3,"Stop");
}
}
void followIR()
{
if (readIR()< 5 && readIR()>0)
{
motor[leftmotor]=-100;
motor[rightmotor]=100;
nxtDisplayTextLine(3,"Left");
}
else if (readIR()> 5)
{
motor[leftmotor]=100;
motor[rightmotor]=100;
nxtDisplayTextLine(3,"Right");
}
else if (readIR()== 5)
{
motor[leftmotor]=100;
motor[rightmotor]=100;
nxtDisplayTextLine(3,"Forward");
}
else
{
motor[leftmotor]=0;
motor[rightmotor]=0;
nxtDisplayTextLine(3,"Stop");
}
}
// Returns true if the IR sensor detects something less than 100cm away
bit findWall()
{
if(readIR() > 100)
return FALSE;
else
return TRUE;
}
void driveUntilIRNot(int irValue, int motorSpeed)
{
while(readIR() == irValue)
{
driveStraight (motorSpeed, 0);
}
}
void driveUntilIR(int irValue, int motorSpeed)//Drives until the IR value equals a preset number.
{
while(readIR() != irValue)
{
driveStraight (motorSpeed, 0);
}
}
void circleRight(void)
{
IR_Read ir;
Data_t data;
List_t *list=NULL;
int i;
ringInit(&list,RINGSIZE);
/* Get initial readings */
for(i=0;i<RINGSIZE;i++)
{
/* Read from IRs */
readIR(&ir);
/* Convert to distance */
convertFullDistance(&ir,&data);
/* Push data onto the ring */
ringPush(&list,data);
delay(20);
}
setSpeed();
while(1)
{
if(i>80)
{
setSpeed();
i=0;
}
readIR(&ir);
convertFullDistance(&ir,&data);
// data.turn_angle = followRight(data.frontRight,data.backRight);
data.turn_angle = left(&data,&list->data);
setSteeringAngle(data.turn_angle);
ringPush(&list,data);
i++;
delay(60);
}
}
task main() {
float pointDistance[] = { 19.0, 10.0, 19.0, 10.0 };
float totalDistance = 0.0;
int i;
startRobot();
waitForStart();
if (PLAY_SOUNDS == true) {
PlaySound(soundBeepBeep);
}
if (WAIT_FOR_START == true) {
wait1Msec(START_DELAY);
}
// Connor's portion
for (i = 0; i < 4; i++) {
if (i == 0 && ALT_ROUTE == true) {
driveDistance(ALT_DISTANCE1, FORWARD, ADJ_NO);
turnRobot(ALT_DEGREES, FORWARD, TURNTYPE_A);
driveDistance(ALT_DISTANCE2, REVERSE, ADJ_NO);
}
else {
driveDistance(pointDistance[i], DIRECTION_A, ADJ_YES);
}
totalDistance += pointDistance[i];
if (QUICK_ROUTE == true || i == 3 || readIR(i+1) == true) {
wait1Msec(250);
servo[blockServo] = 80;
servo[flagServo] = 65;
wait1Msec(500);
servo[blockServo] = 254;
servo[flagServo] = 65;
wait1Msec(250);
break;
}
}
// Devan's portion
if (DIRECTION_A == DIRECTION_B) {
driveDistance(CULM_DISTANCE - totalDistance, DIRECTION_B, ADJ_YES);
}
else {
driveDistance(totalDistance - 6.5, DIRECTION_B, ADJ_NO);
}
turnRobot(TURN_DEGREES_B, DIRECTION_B, TURNTYPE_B);
driveDistance(35.0, DIRECTION_B, ADJ_NO);
turnRobot(TURN_DEGREES_B, DIRECTION_B, TURNTYPE_B);
driveDistance(PARK_DISTANCE, DIRECTION_B, ADJ_NO);
if (PLAY_SOUNDS == true) {
PlaySound(soundBeepBeep);
wait1Msec(500);
}
}
int turnUntilIRNot(int irValue, int motorSpeedTurn)//Turns the robot in place until the IR reads a value that is not equivalent to the parametrically set condition value.
{
while(readIR() == irValue)
{
motor[leftmotor] = motorSpeedTurn;
motor[rightmotor] = -motorSpeedTurn;
}
motor[leftmotor]=0;
motor[rightmotor]=0;
return happy_angle;
}
void MLX90621::measure() {
if (checkConfig()) {
readEEPROM();
writeTrimmingValue();
setConfiguration();
}
readPTAT();
readIR();
calculateTA();
readCPIX();
calculateTO();
}
void goStraightUntilIRNothing(int speed)//This function allows locomotion over a predefined period (per the parameters) and stop. We create a timer in order to measure the time elapsed between the beginning and end of execution of this function.
{
servo[servo2]=127;
int initValue = 0;
int raw = 0;
initValue = LSvalRaw(LightSensor);
raw = initValue;
while(readIR() > 0)//While loop saying that while the value of the light sensor is less than the value of the parametrically set stop value.
{
raw = LSvalRaw(LightSensor);
nxtDisplayCenteredBigTextLine(1, "%d", raw);
driveStraight (speed, 0);//Drivers forward with no light sensor value change (the null value is set in the parameters)
}
}
task main()
{
waitForStart();
// raiseBucket();
int IRState; //Gets value from the IR
//StartTask (Play);
StartTask (update_gyroSensor);
goStraightForTime(4000, 40);
goStraightForTime(500, 20);
goStraightForTime(500, 10);
driveUntilIRNot(5, 40);//Adjust later
IRState = readIR();
driveUntilIR(0, 40);
driveUntilBumper(15);//Adjust later
if(IRState == 4) // if IR is 4 then Turn
{
turnToAngle( -40, -60, -90);
}
else
{
turnToAngle( 40, -60, 90);
}
if(IRState > 5)
{
driveUntilIR(1, 40);
turnToAngle(-40, -0, 0);
driveUntilBumper(15);
}
else if(IRState < 5)
{
driveUntilIR(9, 40);
turnToAngle(40, 0, 0);
driveUntilBumper(15);
}
else
{
driveUntilIR(0,40);
}
//turnToAngle(40, 60, 90);
//driveUntilIR();
}
void printIR() {
// gather data from ports
// may need to swap port1-2-3-4 function inupts with global definitions...
int irData = readIR();
clearLCD();
// to test, without readIR(), use this code...
printCharLCD(!IR1port + '0');
printCharLCD(!IR2port + '0');
printCharLCD(!IR3port + '0');
printCharLCD(!IR4port + '0');
//
// printCharLCD((irData & 1) + '0'); // print first bit
// printCharLCD(((irData & 2) >> 1) + '0'); // print second bit
// printCharLCD(((irData & 4) >> 2) + '0'); // print third bit
// printCharLCD(((irData & 8) >> 3) + '0'); // print fourth bit
}
void testMotorAndIR() {
int data = 0;
data = readIR();
clearLCD();
if (data == 0b1111) {
setMotorsBackward(100);
printStringLCD("backward");
//setMotorsSweepForward(1023);
} else if (data == 0) {
setMotorsForward(100);
printStringLCD("forward");
}
moveCursorLCD(1, 1);
printCharLCD((((data & 8) >> 3) + '0'));
printCharLCD((((data & 4) >> 2) + '0'));
printCharLCD((((data & 2) >> 1) + '0'));
printCharLCD((((data & 1) >> 0) + '0'));
}
void navigation(void){
int i;
IR_Read ir;
Data_t data;
List_t *list=NULL;
ringInit(&list,RINGSIZE);
/* Get initial readings */
for(i=0;i<RINGSIZE;i++)
{
/* Read from IRs */
readIR(&ir);
/* Convert to distance */
convertFullDistance(&ir,&data);
data.trackState = DEFAULT;
/* Push data onto the ring */
ringPush(&list,data);
delay(20);
}
//delay(9000);
setSpeed();
i=0;
while(1)
{
if(i>80)
{
setSpeed();
i=0;
}
data.trackState = DEFAULT;
if(cooldown(list)) turnstate = STRAIGHT;
/* Read from IRs */
readIR(&ir);
/* Convert to distance */
convertFullDistance(&ir,&data);
/* Determine track state */
trackDetection(list,&data);
if(analogRead(AIN4)*0.0114 < 10)
{
digitalWrite(P8_9,HIGH);
}
else
{
digitalWrite(P8_9,LOW);
}
ringPush(&list,data);
if(turnstate==FOLLOWRIGHT)
{
data.turn_angle = right(&data,&list->data);
}
else if(turnstate==FOLLOWLEFT)
{
data.turn_angle = left(&data,&list->data);
}
else if(turnstate==STRAIGHT)
{
data.turn_angle = straight(&data,&list->data);
}
/* Update servo */
setSteeringAngle(data.turn_angle);
// printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
// printf("Front Left Distance: %f, Front Left Sensor: %d\n",data.frontLeft,ir.FrontLeft);
// printf("Front Right Distance: %f, Front Right Sensor: %d\n",data.frontRight,ir.FrontRight);
// printf("Back Left Distance: %f, Back Left Sensor: %d\n",data.backLeft,ir.BackLeft);
// printf("Back Right Distance: %f, Back Right Sensor: %d\n",data.backRight,ir.BackRight);
// printf("Turn angle calculated: %d\n\n",data.turn_angle);
delay(60);
// if(data.trackState==FOLLOWRIGHT) turn_angle=trackStateHandling;
// else printf("Follow left\n");
// printf("Cooldown: %d\n",*cooldown);
i+=1;
}
}
void Sensors::readSensors()
{
readIR();
readMPU();
}
void updateView(WINDOW * win){
int i;
int temp = 0;
int x,y;
y=START_Y;
x=START_X+1;
// print axis row
for(i=0; i<16; i++){
mvwprintw(win,y,x,"%X", temp++);
x+=3;
}
temp = 0;
x = START_X-2;
y=START_Y+1;
// print axis col
for(i=0; i<16; i++){
mvwprintw(win,y,x,"%X", temp++);
y++;
}
y = START_Y;
// Print memory
for(i=0; i<256; i++){
if(i%16==0){
y++;
x=START_X;
}
if(highlight[section] == i && section == SECTION_MEMORY){
wattron(win, A_REVERSE);
mvwprintw(win,y,x,"%.2X", readMemoryAt(m,i));
wattroff(win, A_REVERSE);
}else{
mvwprintw(win,y,x,"%.2X", readMemoryAt(m,i));
}
x+=3;
}
// Print Registers
x = START_X;
y+=2;
for(i=0; i<16; i++){
mvwprintw(win,y,x,"R%X",i );
x+= 3;
}
x = START_X;
y++;
for(i=0; i<16; i++){
if(highlight[section] == i && section == SECTION_REGISTERS){
wattron(win, A_REVERSE);
mvwprintw(win,y,x,"%.2X", readRegistersAt(m,i));
wattroff(win, A_REVERSE);
}else{
mvwprintw(win,y,x,"%.2X", readRegistersAt(m,i));
}
x+= 3;
}
x = START_X;
y += 2;
// print pc and ic
mvwprintw(win,y,x,"PC : ");
if(section == SECTION_PC){
wattron(win, A_REVERSE);
mvwprintw(win,y,x+5,"%.2X", readPC(m));
wattroff(win, A_REVERSE);
}else{
mvwprintw(win,y,x+5,"%.2X", readPC(m));
}
x+= 12;
mvwprintw(win,y,x,"IR : %.4X", readIR(m));
x+= 20;
mvwprintw(win,y,x,"STATUS : ");
if(getStatus(m) == NORMAL){
mvwprintw(win,y,x+9,"NORMAL");
}else if(getStatus(m) == HALTED){
mvwprintw(win,y,x+9,"HALTED");
}
else if(getStatus(m) == ERROR){
mvwprintw(win,y,x+9,"ERROR ");
}
wrefresh(win);
}
