Ben's secondary robot. |
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, HTIRS2, sensorI2CCustom)
#pragma config(Motor, motorA, spinner, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, Wheel_Left, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, Wheel_Right, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C2_1, servo1, tServoNone)
#pragma config(Servo, srvo_S1_C2_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C2_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "drivers/HTIRS2-driver.h"
task spin();
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int _distDCsum = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int total = 0;
int count = 0;
int combinedavg = 0;
nxtDisplayTextLine(1, "Display Error!");
StartTask(spin);
while (true)
{
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break;
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break;
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break;
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break;
_distDCsum = (dcS1 + dcS2 + dcS3 + dcS4 + dcS5);
ClearTimer(T1);
while(time1[T1] < 1600)
{
total = total + _dirDC;
if (_dirDC != 0)
{
count = count + 1;
}
}
int _dirAvgSpin = 0;
_dirAvgSpin = abs(total / count);
combinedavg = (combinedavg + _dirAvgSpin) / count;
nxtDisplayTextLine(4, "DistAvg: %d", _dirAvgSpin);
nxtDisplayTextLine(3, "AverageAvg: %d", combinedavg);
}
}
task spin()
{
while(true)
{
motor[spinner] = 15;
wait1Msec(800);
motor[spinner] = -15;
wait1Msec(800);
}
}