Robot-C

main.c

#pragma config(Sensor, S1, IR, sensorI2CCustom)
//#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
//#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed)

/*
TO-DO
- find real value of timeEnd
- find motor strengths
- find left and right motors
*/

int direction = 0, modDIR = 0;
int timer = 0;
int timeEnd = 5000;
int motorD = motorA;
int motorE = motorC;
int lineup = 2500;

#include "C:\Program Files\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-irseeker-v2.h";
void nxtlog(string output) {
  nxtDisplayCenteredTextLine(3, output);
}
void goForward() {
		int power = 20;
    //goes forward until 130d to Sensor
    while (direction != 3 && direction != 7) {
    		nxtDisplayCenteredTextLine(3, "direction: %d", direction);
        motor[motorE] = power;
        motor[motorD] = power;
        wait1Msec(50);
        direction = HTIRS2readDCDir(IR);
        timer = timer + 50;
    }
    wait1Msec(lineup);
    
    motor[motorE] = 0;
    motor[motorD] = 0;
    
    wait1Msec(2000);
}
void turnToFaceRack() {
    //turns to be perpendicular
    timer = 0;
    if (direction == 3){
        //turn left
        while (direction != 5) {
            motor[motorE] =	5;
            motor[motorD] = -5;
    //        wait1Msec(100);
            direction = HTIRS2readDCDir(IR);
        }
    } else if (direction == 7){
        //turn right
    		while (direction != 5) {
						motor[motorE] =	-5;
						motor[motorD] = 5;
		//				wait1Msec(100);
						direction = HTIRS2readDCDir(IR);
        }
    }
    
    motor[motorE] = 0;
    motor[motorD] = 0;
    
    wait1Msec(2000);
}
void approachRack() {
    //moves closer to ringstand
    while (HTIRS2readDCAverage(IR) < 40 ) {
        motor[motorE] =	20;
        motor[motorD] = 20;
        wait1Msec(50);
        direction = HTIRS2readDCDir(IR);
    }
    
    motor[motorE] = 0;
    motor[motorD] = 0;
    
    wait1Msec(2000);
}
void riseToHeight() {
    //sets lift to right height
    motor[motorB] = -100;
    wait1Msec(5000);
    motor[motorB] = 0;
    
    wait1Msec(2000);
}
void placeRing() {
    //moves ring onto ringstand
    while (HTIRS2readDCAverage(IR) < 100 ) {
        motor[motorE] =	20;
        motor[motorD] = 20;
        wait1Msec(50);
        direction = HTIRS2readDCDir(IR);
    }
    
    motor[motorE] = 0;
    motor[motorD] = 0;
    
    wait1Msec(2000);
}
void returnLift() {
    //drops lift a bit
    motor[motorB] = 20;
    wait1Msec(200);
    motor[motorB] = 0;
}
void goBackward() {
    //backs up a bit
    motor[motorE] = -40;
    motor[motorD] = -40;
    wait1Msec(2000);
    motor[motorE] = 0;
    motor[motorD] = 0;
}
task main()
{
	string out = "text";
	nxtlog(out);
	tHTIRS2DSPMode _mode = DSP_1200;

	goForward();
	out = "Went initial distance.";
	nxtlog(out);
	turnToFaceRack();
	out = "Faced rack.";
	nxtlog(out);
	approachRack();
	out = "Arrived at rack.";
	nxtlog(out);
	riseToHeight();
	out = "Elevated lift.";
	nxtlog(out);
	placeRing();
	out = "Placed ring.";
	nxtlog(out);
	returnLift();
	out = "Lowered lift.";
	nxtlog(out);
	//goBackward();
	out = "Left rack.";
	nxtlog(out);
}