Sensor createSensor(SensorType type, int port) {
static bool initIME = false;
Sensor retSen = {.type = type, .port = port};
switch (type) {
case SHAFT_ENCODER:
retSen.enc = encoderInit(port, port + 1, false);
if (DEBUG) {
print("encoder init");
if (retSen.enc == NULL) {
print(" FAILED");
}
getchar();
}
break;
case GYROSCOPE:
retSen.gyr = gyroInit(port, 0);
break;
case ULTRASONIC:
retSen.ult = ultrasonicInit(port, port + 1);
break;
case IME:
if (!initIME) {
printDebug("Init IME!");
// imeReinitialize();
initIME = true;
printDebug("Done");
}
break;
default:
break;
}
return retSen;
}
//*****************************************************************************
// Start Main
//*****************************************************************************
int main(void) {
char data_in[128];
//***************************************************************************
// Setting the system Clock
//
// 400 MHz PLL driven by 16 MHz external crystal
// builtin /2 from PLL, then /4 --> 50Hz Clock speed
SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
//
//***************************************************************************
InitUART3();
MotorInit(); //Initialize Motors
LineInit(); //Initialize Line Sensors
ultrasonicInit(); //Initialize UltraSonic
IRdsitinit(); //Initialize IR Distance Sensor
colorinit(); //Initialize Color Sensor
FollowINIT(); //Initialize timer interrupts for following
ShaftEncoder_Init();
IntMasterEnable(); //Enable Interrupts
UARTprintf("An & Paolina's Robot says Hello\n\r");
while (1) {
UARTgets(data_in,128);
MB_Get((unsigned char *)data_in);
Interpret(data_in);
}
}
//sensorConfig is used to set the multiplier for the gyro sensor
//if that is set as the sensor type. otherwise it does nothing.
//Call imeInitializeAll() before calling this function
Sensor sensorInit(SensorType type, SensorPort port_1, SensorPort port_2,
bool inverted) {
Sensor sensor;
Sensor *empty = NULL;
sensor.type = type;
if (sensor.port_1 >= Analog_1 && sensor.port_1 <= Analog_8
&& (type != Sensor_Bumper && type != Sensor_LimitSwitch)) {
sensor.isAnalog = true;
} else {
sensor.isAnalog = false;
}
sensor.port_1 = port_1;
sensor.port_2 = port_2;
sensor.inverted = inverted;
if (sensor.isAnalog) {
sensor.inverted = false;
}
switch (sensor.type) {
case Sensor_QuadEncoder:
sensor.sensorData.quadEncoder = quadEncoderInit(sensor.port_1,
sensor.port_2, inverted);
break;
case Sensor_Sonar:
sensor.sensorData.sonar = ultrasonicInit(sensor.port_1, sensor.port_2);
break;
case Sensor_Line:
sensor.sensorData.analog = analogSensorInit(sensor.port_1);
break;
case Sensor_Light:
sensor.sensorData.analog = analogSensorInit(sensor.port_1);
break;
case Sensor_Bumper:
sensor.sensorData.pushButton = pushButtonInit(sensor.port_1, inverted);
break;
case Sensor_LimitSwitch:
sensor.sensorData.pushButton = pushButtonInit(sensor.port_1, inverted);
break;
case Sensor_Potentiometer:
sensor.sensorData.analog = analogSensorInit(sensor.port_1);
break;
case Sensor_Gyroscope:
sensor.sensorData.analog = analogSensorInit(sensor.port_1);
break;
case Sensor_Accelerometer:
sensor.sensorData.analog = analogSensorInit(sensor.port_1);
break;
default:
print("Invalid Sensor Type\n\r");
return *empty;
}
return sensor;
}
void vTaskUltraSonic()
{
Ultrasonic us = ultrasonicInit(ULTRASONIC_ECHO_CHANNEL, ULTRASONIC_PING_CHANNEL);
unsigned long nextWakeTime;
while (1)
{
nextWakeTime = millis();
//get reading from sensor
usValue = ultrasonicGet(us);
fprintf(stdout,"Ultrasonc value = %d\n", usValue);
taskDelayUntil(&nextWakeTime, 50);
}
}
Drive initDrive(int leftMotor, int leftMotorInverted,
int rightMotor, int rightMotorInverted, int leftEncoderTopPort,
int leftEncoderBottomPort, int leftEncoderInverted,
int rightEncoderTopPort, int rightEncoderBottomPort,
int rightEncoderInverted, int gyroPort, int echoPort,
int pingPort)
{
PantherMotor left = initPantherMotor(leftMotor, leftMotorInverted);
PantherMotor right = initPantherMotor(rightMotor, rightMotorInverted);
Encoder leftEncoder = encoderInit(leftEncoderTopPort,
leftEncoderBottomPort, leftEncoderInverted);
Encoder rightEncoder = encoderInit(rightEncoderTopPort,
rightEncoderBottomPort, rightEncoderInverted);
Gyro gyro = gyroInit(gyroPort, 0);
Ultrasonic newSonar = ultrasonicInit(echoPort, pingPort);
Drive newDrive = {left, right, leftEncoder, rightEncoder, gyro, newSonar};
return newDrive;
}
