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; }