/*Aplicación en la que se encienden y apagan todos los leds de la placa,
* de modo secuencial.*/
int main(void)
{
tec.T1 = 1;
tec.T2 = 1;
tec.T3 = 1;
tec.T4 = 1;
rgb.blue = 1;
rgb.green = 1;
rgb.red = 1;
uint32_t i;
#define DELAY 5000000
InitGPIO_EDUCIAA_Teclas();
InitGPIO_EDUCIAA_Leds();
while(1)
{
Escaneo_Teclado(&tec);
if (tec.T1 == 0)
{
rgb.blue=1;
rgb.green=1;
rgb.red=1;
ToggleLedRGB(&rgb);
}
else
{
rgb.blue=0;
rgb.green=0;
rgb.red=0;
RGBoff(&rgb);
}
if (tec.T2 == 0)
ToggleLed(LED_AMARILLO);
else
LEDoff(LED_AMARILLO);
if (tec.T3 == 0)
ToggleLed(LED_ROJO);
else
LEDoff(LED_ROJO);
if (tec.T4 == 0)
ToggleLed(LED_VERDE);
else
LEDoff(LED_VERDE);
for(i = DELAY; i != 0; i--)
{
asm("nop");
}
}
}
void main (void)
{
initialize();
wait1Msec(200);
//initialize values
int pwr = 15;
int sensorState = -1;
//determine these values by running the calibration program first
int threshold1 = 150;
int threshold2 = 150;
int delay = 10;
//initialize motors and LED indicators
setMotor (MOTOR_A, 0);
setMotor (MOTOR_B, 0);
LEDnum(0);
LEDoff(RED_LED);
LEDoff(GREEN_LED);
//waits for a button press
while(getSensor(BUTTON) == 0);
wait1Msec(100);
while(getSensor(BUTTON) == 1);
resetTimer();
while(getSensor(BUTTON)== 0 && time100() < 64) {
//line follows for 6.4 seconds
if (sensorState != getSensorState(threshold1, threshold2)) {
//only change direction if conditions are different than before.
sensorState = getSensorState(threshold1,threshold2);
if (sensorState == 0) { //go straight
drive(15, delay);
} else if (sensorState == 1){
rightTurn(15,delay);
} else if (sensorState == 2){
leftTurn(15, delay);
} else if (sensorState == 3){
rightTurn(15, delay);
}
}
}
//hard code exit 2
rightTurn(pwr,2400);
drive(pwr,9400);
leftTurn(pwr,1500);
drive(pwr,20000);
}
//will be call from IDLE thread
void vApplicationIdleHook( void ){
//just show im alive
static uint8_t led = 0;
if(led)
LEDon(LED_GREEN);
else
LEDoff(LED_GREEN);
led = !led;
}
void main()
{
initialize();
while (getSensor(BUTTON) != 1){}
wait1Msec(200);
while (getSensor(BUTTON) != 0){}
LEDon(RED_LED);
LEDoff(GREEN_LED);
for (int i = 0; i < 8; i++)
{
LEDnum(i);
wait1Msec(500);
}
}
//
//Toggle LED
//
int TLED(){
//Toggle LED
LEDfile = fopen("/sys/class/gpio/gpio49/value","r");
fscanf(LEDfile, "%d", &LED);
//Close file
fclose(LEDfile);
//if LED is on, turn off
if(LED == 1){
LEDoff();
}
//else turn LED on
else{
LEDon();
}
//Call Ptime to print the time
Ptime();
return 0;
}
void configSave(void)
{
uint8_t i;
LEDon();
for (i = 0; i < CONFIGDATASIZE; i++)
{
//read data from eeprom,
//check, if it has changed, only then rewrite;
int data = ReadFromEEPROM(i);
if (data != -1 && data != configData[i])
{
WriteToEEPROM(i, configData[i]);
}
Delay_ms(5);
}
LEDoff();
}
//--------------------Engine Process-----------------------------//
void engineProcess(float dt)
{
static int loopCounter;
tStopWatch sw;
loopCounter++;
LEDon();
DEBUG_LEDoff();
StopWatchInit(&sw);
MPU6050_ACC_get(AccData); // Getting Accelerometer data
unsigned long tAccGet = StopWatchLap(&sw);
MPU6050_Gyro_get(GyroData); // Getting Gyroscope data
unsigned long tGyroGet = StopWatchLap(&sw);
Get_Orientation(AccAngleSmooth, CameraOrient, AccData, GyroData, dt);
unsigned long tAccAngle = StopWatchLap(&sw);
// if we enable RC control
if (configData[9] == '1')
{
// Get the RX values and Averages
Get_RC_Step(Step, RCSmooth); // Get RC movement on all three AXIS
Step[PITCH] = Limit_Pitch(Step[PITCH], CameraOrient[PITCH]); // limit pitch to defined limits in header
}
// Pitch adjustments
//pitch_setpoint += Step[PITCH];
pitchRCOffset += Step[PITCH] / 1000.0;
pitch_angle_correction = constrain((CameraOrient[PITCH] + pitchRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
pitch_setpoint += pitch_angle_correction; // Pitch return to zero after collision
// Roll Adjustments
//roll_setpoint += Step[ROLL];
rollRCOffset += Step[ROLL] / 1000.0;
// include the config roll offset which is scaled to 0 = -10.0 degrees, 100 = 0.0 degrees, and 200 = 10.0 degrees
roll_angle_correction = constrain((CameraOrient[ROLL] + rollRCOffset + Deg2Rad((configData[11] - 100) / 10.0)) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
roll_setpoint += roll_angle_correction; //Roll return to zero after collision
// if we enabled AutoPan on Yaw
if (configData[10] == '0')
{
//ADC1Ch13_yaw = ((ADC1Ch13_yaw * 99.0) + ((float)(readADC1(13) - 2000) / 4000.0)) / 100.0; // Average ADC value
//CameraOrient[YAW] = CameraOrient[YAW] + 0.01 * (ADC1Ch13_yaw - CameraOrient[YAW]);
yaw_setpoint = autoPan(Output[YAW], yaw_setpoint);
}
else
{
// Yaw Adjustments
yaw_setpoint += Step[YAW];
yawRCOffset += Step[YAW] / 1000.0;
}
#if 0
yaw_angle_correction = constrain((CameraOrient[YAW] + yawRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
yaw_setpoint += yaw_angle_correction; // Yaw return to zero after collision
#endif
unsigned long tCalc = StopWatchLap(&sw);
pitch_PID();
roll_PID();
yaw_PID();
unsigned long tPID = StopWatchLap(&sw);
unsigned long tAll = StopWatchTotal(&sw);
printcounter++;
//if (printcounter >= 500 || dt > 0.0021)
if (printcounter >= 500)
{
if (debugPrint)
{
print("Loop: %7d, I2CErrors: %d, angles: roll %7.2f, pitch %7.2f, yaw %7.2f\r\n",
loopCounter, I2Cerrorcount, Rad2Deg(CameraOrient[ROLL]),
Rad2Deg(CameraOrient[PITCH]), Rad2Deg(CameraOrient[YAW]));
}
if (debugSense)
{
print(" dt %f, AccData: %8.3f | %8.3f | %8.3f, GyroData %7.3f | %7.3f | %7.3f \r\n",
dt, AccData[X_AXIS], AccData[Y_AXIS], AccData[Z_AXIS], GyroData[X_AXIS], GyroData[Y_AXIS], GyroData[Z_AXIS]);
}
if (debugPerf)
{
print("idle: %5.2f%%, time[µs]: attitude est. %4d, IMU acc %4d, gyro %4d, angle %4d, calc %4d, PID %4d\r\n",
GetIdlePerf(), tAll, tAccGet, tGyroGet, tAccAngle, tCalc, tPID);
}
if (debugRC)
{
print(" RC2avg: %7.2f | RC3avg: %7.2f | RC4avg: %7.2f | RStep:%7.3f PStep: %7.3f YStep: %7.3f\r\n",
RCSmooth[ROLL], RCSmooth[PITCH], RCSmooth[YAW], Step[ROLL], Step[PITCH], Step[YAW]);
}
if (debugOrient)
{
print("Roll_setpoint:%12.4f | Pitch_setpoint:%12.4f | Yaw_setpoint:%12.4f\r\n",
roll_setpoint, pitch_setpoint, yaw_setpoint);
}
if (debugCnt)
{
print("Counter min %3d, %3d, %3d, max %4d, %4d, %4d, count %3d, %3d, %3d, usbOverrun %4d\r\n",
MinCnt[ROLL], MinCnt[PITCH], MinCnt[YAW],
MaxCnt[ROLL], MaxCnt[PITCH], MaxCnt[YAW],
IrqCnt[ROLL], IrqCnt[PITCH], IrqCnt[YAW],
usbOverrun());
}
if (debugAutoPan)
{
print("Pitch_output:%3.2f | Roll_output:%3.2f | Yaw_output:%3.2f | centerpoint:%4.4f\n\r",
Output[PITCH],
Output[ROLL],
Output[YAW],
centerPoint);
}
printcounter = 0;
}
LEDoff();
}
int main(void) {
// Make sure our watchdog timer is disabled!
wdt_reset();
MCUSR &= ~(1 << WDRF);
wdt_disable();
// Start up the USART for serial communications
// 25 corresponds to 38400 baud - see datasheet for more values
USART_Init(25);// 103 corresponds to 9600, 8 corresponds to 115200 baud, 3 for 250000
// set the prescale for the USB for our 16 MHz clock
CPU_PRESCALE(0);
// Initialize our USB connection
usb_init();
while (!usb_configured()){
LEDon(TXLED);
_delay_ms(50);
LEDoff(TXLED);
_delay_ms(50);
} // wait
// Wait an extra second for the PC's operating system to load drivers
// and do whatever it does to actually be ready for input
// This wait also gives the Arduino bootloader time to timeout,
// so the serial data you'll be properly aligned.
_delay_ms(500);
dataForController_t dataToSend;
char buttonData1;
char buttonData2;
char buttonData3;
while (1) {
// Delay so we're not going too fast
_delay_ms(10);
// We get our data from the ATmega328p by writing which byte we
// want from the dataForController_t, and then wait for the
// ATmega328p to send that back to us.
// The serialRead(number) function reads the serial port, and the
// number is a timeout (in ms) so if there's a transmission error,
// we don't stall forever.
LEDon(TXLED);
flushSerialRead();
serialWrite(0);
buttonData1 = serialRead(25);
serialWrite(1);
buttonData2 = serialRead(25);
serialWrite(2);
buttonData3 = serialRead(25);
serialWrite(3);
dataToSend.leftStickX = serialRead(25);
serialWrite(4);
dataToSend.leftStickY = serialRead(25);
serialWrite(5);
dataToSend.rightStickX = serialRead(25);
serialWrite(6);
dataToSend.rightStickY= serialRead(25);
serialWrite(7);
dataToSend.centerStickX = serialRead(25);
serialWrite(8);
dataToSend.centerStickY= serialRead(25);
LEDoff(TXLED);
// Now, we take the button data we got in and input that information
// into our controller data we want to send
dataToSend.triangleOn = 1 & (buttonData1 >> 0);
dataToSend.circleOn = 1 & (buttonData1 >> 1);
dataToSend.squareOn = 1 & (buttonData1 >> 2);
dataToSend.crossOn = 1 & (buttonData1 >> 3);
dataToSend.l1On = 1 & (buttonData1 >> 4);
dataToSend.l2On = 1 & (buttonData1 >> 5);
dataToSend.l3On = 1 & (buttonData1 >> 6);
dataToSend.r1On = 1 & (buttonData1 >> 7);
dataToSend.r2On = 1 & (buttonData2 >> 0);
dataToSend.r3On = 1 & (buttonData2 >> 1);
dataToSend.selectOn = 1 & (buttonData2 >> 2);
dataToSend.startOn = 1 & (buttonData2 >> 3);
dataToSend.homeOn = 1 & (buttonData2 >> 4);
dataToSend.dpadLeftOn = 1 & (buttonData2 >> 5);
dataToSend.dpadUpOn = 1 & (buttonData2 >> 6);
dataToSend.dpadRightOn = 1 & (buttonData2 >> 7);
dataToSend.dpadDownOn = 1 & (buttonData3 >> 0);
// Finally, we send the data out via the USB port
sendPS3Data(dataToSend);
}
}
void main()
{
initialize();
wait1Sec(2);
LEDon(RED_LED);
LEDoff(GREEN_LED);
LEDnum(3);
volatile int x = 0;
while (getSensor(BUTTON) != 1){}
wait1Msec(200);
while (getSensor(BUTTON) != 0){}
LEDon(GREEN_LED);
LEDoff(RED_LED);
LEDnum(3);
while (getSensor(BUMPER) != 1)
{
if (getSensor(REFLECT_2) <0)
LEDnum(0);
else if (getSensor(REFLECT_2)<75)
{
LEDnum(1);
setMotor(MOTOR_A, 10);
}
else if (getSensor(REFLECT_2)<150)
LEDnum(2);
else if (getSensor(REFLECT_2)<175)
LEDnum(3);
else if (getSensor(REFLECT_2)<200)
LEDnum(4);
else if (getSensor(REFLECT_2)<250)
LEDnum(5);
else if (getSensor(REFLECT_2)<300)
LEDnum(6);
else if (getSensor(REFLECT_2)>300)
LEDnum(7);
}
wait1Msec(200);
while (getSensor(BUMPER) != 0);
wait1Msec(200);
while (getSensor(BUMPER) != 1)
{
if (getSensor(REFLECT_1) <0)
LEDnum(0);
else if (getSensor(REFLECT_1)<75)
LEDnum(1);
else if (getSensor(REFLECT_1)<150)
LEDnum(2);
else if (getSensor(REFLECT_1)<175)
LEDnum(3);
else if (getSensor(REFLECT_1)<200)
LEDnum(4);
else if (getSensor(REFLECT_1)<250)
LEDnum(5);
else if (getSensor(REFLECT_1)<300)
LEDnum(6);
else if (getSensor(REFLECT_1)>300)
LEDnum(7);
}
wait1Msec(200);
while (getSensor(BUMPER) != 0);
}
//////////////////////////////////////////////////
//Main
//////////////////////////////////////////////////
int main()
{
//While loop to run program indefinately
while(1){
//LIGHT SENSOR HANDLING/////////////////////////////
//Open file and read Sensor Signal
Ain0 = fopen("/sys/bus/iio/devices/iio:device0/in_voltage0_raw","r");
fscanf(Ain0,"%d",&Ain);
//Check if Sensor has switched states
if(((AinComp-Ain)<-500)|(AinComp-Ain)>500)){
SensorTrig = 1;
}
//Check if light is ON or OFF
if(SensorTrig==1){
//Determine if HIGH or LOW
// NOTE: Light off when value below 500
//check if light is on
if(Ain > 500){
printf("Light was turned on.\n");
LEDoff();
AinComp=Ain;
SensorTrig=0;
}
//If light is not on it is OFF
else{
printf("Light was turned off.\n");
LEDon();
AinComp=Ain;
SensorTrig=0;
}
}
fclose(Ain0);
//BUTTON PRESS HANDLING/////////////////////////////
//Open file to check newstate of button
Button = fopen("/sys/class/gpio/gpio115/value","r");
fscanf(Button,"%d",&Newstate);
//Check if button is pressed
if((Oldstate==0)&(Newstate==1){
Buttonpress = 1;
}
//Reset Oldsate variable to Newstate
Oldstate = Newstate;
//If button is pressed call toggle LED
if(Buttonpress==1){
//Call toggle LED
TLED();
//Reset Buttonpress variable
Buttonpress = 0;
}
//Close file for Button
fclose(Button);
}
return 0;
}
void turnOffLEDs() {
LEDoff(LED_GREEN);
LEDoff(LED_RED);
}
int main(void) {
// Make sure our watchdog timer is disabled!
wdt_reset();
MCUSR &= ~(1 << WDRF);
wdt_disable();
// Start up the USART for serial communications
// 25 corresponds to 38400 baud - see datasheet for more values
USART_Init(25);// 103 corresponds to 9600, 8 corresponds to 115200 baud, 3 for 250000
// set the prescale for the USB for our 16 MHz clock
CPU_PRESCALE(0);
// Initialize our USB connection
usb_init();
while (!usb_configured()){
LEDon(TXLED);
_delay_ms(50);
LEDoff(TXLED);
_delay_ms(50);
} // wait
// Wait an extra second for the PC's operating system to load drivers
// and do whatever it does to actually be ready for input
// This wait also gives the Arduino bootloader time to timeout,
// so the serial data you'll be properly aligned.
_delay_ms(500);
dataForMegaController_t controllerData1;
dataForMegaController_t controllerData2;
while (1) {
// Delay so we're not going too fast
_delay_ms(10);
// We get our data from the ATmega328p by writing which byte we
// want from the dataForController_t, and then wait for the
// ATmega328p to send that back to us.
// The serialRead(number) function reads the serial port, and the
// number is a timeout (in ms) so if there's a transmission error,
// we don't stall forever.
LEDon(TXLED);
flushSerialRead();
int serialIndex = 0;
// The buttons are held in an array, so we need to break it between the two controllers
for (int i = 0; i < BUTTON_ARRAY_LENGTH; i++){
serialWrite(serialIndex);
serialIndex++;
controllerData1.buttonArray[i] = serialRead(25);
}
for (int i = 0; i < BUTTON_ARRAY_LENGTH; i++){
serialWrite(serialIndex);
serialIndex++;
controllerData2.buttonArray[i] = serialRead(25);
}
serialWrite(serialIndex);
serialIndex++;
uint8_t directionButtons = serialRead(25);
controllerData1.dpadLeftOn = 1 & (directionButtons >> 0);
controllerData1.dpadUpOn = 1 & (directionButtons >> 1);
controllerData1.dpadRightOn = 1 & (directionButtons >> 2);
controllerData1.dpadDownOn = 1 & (directionButtons >> 3);
controllerData2.dpadLeftOn = 1 & (directionButtons >> 4);
controllerData2.dpadUpOn = 1 & (directionButtons >> 5);
controllerData2.dpadRightOn = 1 & (directionButtons >> 6);
controllerData2.dpadDownOn = 1 & (directionButtons >> 7);
// Assuming that 16 bit data gets sent high byte first
controllerData1.leftStickX = get16bitValue(serialIndex);
serialIndex += 2;
controllerData1.leftStickY = get16bitValue(serialIndex);
serialIndex += 2;
controllerData1.rightStickX = get16bitValue(serialIndex);
serialIndex += 2;
controllerData1.rightStickY = get16bitValue(serialIndex);
serialIndex += 2;
controllerData1.stick3X = get16bitValue(serialIndex);
serialIndex += 2;
controllerData1.stick3Y = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.leftStickX = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.leftStickY = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.rightStickX = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.rightStickY = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.stick3X = get16bitValue(serialIndex);
serialIndex += 2;
controllerData2.stick3Y = get16bitValue(serialIndex);
// Communication with the Arduino chip is over here
LEDoff(TXLED);
// Finally, we send the data out via the USB port
sendControllerDataViaUSB(controllerData1, 1);
_delay_ms(10);
sendControllerDataViaUSB(controllerData2, 2);
}
}
/*
Drive function has automation and PID control logic for all 3 courses.
*/
void drive(int courseNumber) {
if (courseNumber == 1) {
// Course #1 - Version 1
int threshhold = 90;
while (true) {
setMotor(MOTOR_A, 100);
setMotor(MOTOR_B, 100);
if (getSensor(REFLECT_1) < threshhold) {
setMotor(MOTOR_A, 0);
while (getSensor(REFLECT_1) < threshhold);
}
if (getSensor(REFLECT_2) < threshhold) {
setMotor(MOTOR_B, 0);
while (getSensor(REFLECT_2) < threshhold);
}
}
} else if (courseNumber == 2) {
// Course #1 - Version 2 (PID CONTROL)
int imotor_power = 100;
float error = getSensor(REFLECT_2) - getSensor(REFLECT_1),
error_prev = error,
error_sum = 0,
kp = 0.9,
ki = 0.0,
kd = 3,
PID_power;
while (true) {
while (getSensor(BUTTON) == 0) {
int A = getSensor(REFLECT_2),
B = getSensor(REFLECT_1);
if (A > 150)
A = 150;
if (B > 150)
B = 150;
error = B - A;
PID_power = kp * error + ki * error_sum + kd * (error - error_prev);
setMotor(MOTOR_A, int(floor(imotor_power + PID_power)));
setMotor(MOTOR_B, int(floor(imotor_power - PID_power)));
wait1Msec(25);
error_prev = error;
error_sum += error;
}
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 0);
while (getSensor(BUTTON) == 1) {}
while (getSensor(BUTTON) == 0) {}
while (getSensor(BUTTON) == 1) {}
}
} else if (courseNumber == 3) {
// Course #2 and #3 (CURVE METHOD)
int step = 100;
while (true) {
setMotor(MOTOR_A, 100);
setMotor(MOTOR_B, step);
if (getSensor(BUTTON) == 1) {
LEDoff(RED_LED);
step -= 1;
while (getSensor(BUTTON) == 1);
LEDon(RED_LED);
}
wait1Msec(80);
}
} else {
// Invalid course number. Terminate.
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 0);
}
}