void checkLEDsSettingButtons() {
if (isButtonPressed(SB4)) {
LEDon(LED_GREEN);
}
if (isButtonPressed(SB3)) {
LEDon(LED_RED);
}
}
//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;
}
int main(void)
{
LEDS_init();
LEDon(LED_BLUE);
ws2811_init();
clearAllLED();
//xTaskCreate(animation_task, (signed char*)"animation", 16, 0, 1, 0);
//vTaskStartScheduler();
animation_task((void *) 0);
return -1;
}
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();
}
void CommHandler(void) //UART4 Interrupt handler implementation
{
int c = GetChar();
if (c >= 0)
{
LEDon();
switch (c)
{
/*
case 'a':
debugAutoPan ^= 1;
print("Autopan messages %s\r\n", debugAutoPan ? "on" : "off");
break;
case 'b':
print("rebooting into boot loader ...\r\n");
Delay_ms(1000);
bootloader();
break;
case 'c':
debugCnt ^= 1;
print("counter messages %s\r\n", debugCnt ? "on" : "off");
break;
case 'd':
debugPrint ^= 1;
print("debug messages %s\r\n", debugPrint ? "on" : "off");
break;
case 'g':
Delay_ms(100);
PutChar('x');
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = configData[i];
PutChar(data);
//print("\\%c\\",(char)data);
}
break;
case 'G':
printConfig();
break;
#if 0
case 'H':
if (CharAvailable() >= CONFIGDATASIZE)
{
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = GetChar();
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
}
else
{
UnGetChar(c); // try again in next loop
}
break;
#endif
case 'h':
for (int i = 0; i < CONFIGDATASIZE; i++)
{
int data;
while ((data = GetChar()) < 0)
;
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
break;
case 'i':
ConfigMode = 1;
break;
case 'j':
ConfigMode = 0;
break;
case 'o':
debugOrient ^= 1;
print("Orientation messages %s\r\n", debugOrient ? "on" : "off");
break;
case 'p':
debugPerf ^= 1;
print("performance messages %s\r\n", debugPerf ? "on" : "off");
break;
case 'r':
debugRC ^= 1;
print("RC messages %s\r\n", debugRC ? "on" : "off");
break;
case 'R':
print("rebooting...\r\n");
Delay_ms(1000);
reboot();
break;
case 's':
debugSense ^= 1;
print("Sensor messages %s\r\n", debugSense ? "on" : "off");
break;
case 'u':
{
extern int bDeviceState;
printUSART("\r\nYY bDeviceState %3d VCPConnectMode %d\r\n", bDeviceState, GetVCPConnectMode());
break;
}
case 'v':
print("Version: %s\r\n", __EV_VERSION);
break;
case '+':
testPhase += 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
case '-':
testPhase -= 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
case '~': {
debugLAKITU ^= 1;
print("Angle messages %s\r\n", debugLAKITU ? "on" : "off");
} break;
case '`':{
LAKITU_enable ^= 1;
print("Turning LAKITU control %s\r\n", LAKITU_enable ? "on" : "off");
} break;
*/
case '$': {
if (CharAvailable() >= LAKITU_LENGTH){
float tmp1 =0.0f, tmp2=0.0f, tmp3= 0.0f;
for (int i = 0; i < LAKITU_LENGTH; i++){
float tmp_val = (float)(((int)GetChar())-ASCII2INT);
if(i<3){
if(i==0){
tmp1 += tmp_val*100.0;
}else if(i==1){
tmp1 += tmp_val*10.0;
}else{
tmp1 += tmp_val*1.0;
}
}else if(i>2 && i<6){
if(i==3){
tmp2 += tmp_val*100.0;
}else if(i==4){
tmp2 += tmp_val*10.0;
}else{
tmp2 += tmp_val*1.0;
}
}else if(i>5 && i<9){
if(i==6){
tmp3 += tmp_val*100.0;
}else if(i==7){
tmp3 += tmp_val*10.0;
}else{
tmp3 += tmp_val*1.0;
}
}else if(i==9 && (int)tmp_val == 11){//handle last character
if ((tmp1 >= 30.0) && (tmp1 <= 140.0)){
LakituAngles[PITCH] = tmp1;
}
if ((tmp2 >= 0.0) && (tmp2 <= 180.0)){
LakituAngles[ROLL] = tmp2;
}
if ((tmp3 >= 0.0) && (tmp3 <= 360.0)){
LakituAngles[YAW] = tmp3;
}
}
}
//print("%f,%f,%f,%f,%f,%f\n",tmp1,tmp2,tmp3,LakituAngles[PITCH],LakituAngles[ROLL],LakituAngles[YAW]);
} else {
UnGetChar(c); // try again in next loop
}
} break;
/*
case '?':
print("CLI documentation\r\n");
// print("\t'+' test phase output increase (now %5.1f)\r\n", testPhase);
//print("\t'-' test phase output decrease (now %5.1f)\r\n", testPhase);
print("\t'a' autopan messages display (now %s)\r\n", debugAutoPan ? "on" : "off");
print("\t'b' reboot into bootloader\r\n");
print("\t'c' counter messages display (now %s)\r\n", debugCnt ? "on" : "off");
print("\t'd' debug messages display (now %s)\r\n", debugPrint ? "on" : "off");
print("\t'g' dump configuration (binary)\r\n");
print("\t'G' dump configuration (hexadecimal)\r\n");
// print("\t'h' write and save config array\r\n");
print("\t'i' enter config mode (now %s)\r\n", ConfigMode ? "on" : "off");
print("\t'j' leave config mode (now %s)\r\n", ConfigMode ? "on" : "off");
print("\t'o' orientation messages display (now %s)\r\n", debugOrient ? "on" : "off");
print("\t'p' performance messages display (now %s)\r\n", debugPerf ? "on" : "off");
print("\t'r' RC messages display (now %s)\r\n", debugRC ? "on" : "off");
print("\t'R' reboot\r\n");
print("\t's' toggle sensor messages display (now %s)\r\n", debugSense ? "on" : "off");
print("\t'u' print USB state (bDeviceState %3d VCPConnectMode %d)\r\n", bDeviceState, GetVCPConnectMode());
print("\t'v' print version (%s)\r\n", __EV_VERSION);
break;
*/
default:
// TODO
break;
}
}
}
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);
}
}
int main(void){
initLEDs();
LEDon(LED_GREEN);
return 0;
}
void CommHandler(void) //UART4 Interrupt handler implementation
{
int c = GetChar();
if (c >= 0)
{
LEDon();
switch (c)
{
case 'a':
debugAutoPan ^= 1;
print("Autopan messages %s\r\n", debugAutoPan ? "on" : "off");
break;
case 'b':
print("rebooting into boot loader ...\r\n");
Delay_ms(1000);
bootloader();
break;
case 'c':
debugCnt ^= 1;
print("counter messages %s\r\n", debugCnt ? "on" : "off");
break;
case 'd':
debugPrint ^= 1;
print("debug messages %s\r\n", debugPrint ? "on" : "off");
break;
case 'g':
Delay_ms(100);
PutChar('x');
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = configData[i];
PutChar(data);
}
break;
case 'G':
printConfig();
break;
#if 0
case 'H':
if (CharAvailable() >= CONFIGDATASIZE)
{
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = GetChar();
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
}
else
{
UnGetChar(c); // try again in next loop
}
break;
#endif
case 'h':
for (int i = 0; i < CONFIGDATASIZE; i++)
{
int data;
while ((data = GetChar()) < 0)
;
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
break;
case 'i':
ConfigMode = 1;
break;
case 'j':
ConfigMode = 0;
break;
case 'o':
debugOrient ^= 1;
print("Orientation messages %s\r\n", debugOrient ? "on" : "off");
break;
case 'O':
debugGravityVector ^= 1;
print("GVector messages %s\r\n", debugGravityVector ? "on" : "off");
break;
case 'S':
debugSetpoints ^= 1;
print("Setpoints messages %s\r\n", debugSetpoints ? "on" : "off");
break;
case 'p':
debugPerf ^= 1;
print("performance messages %s\r\n", debugPerf ? "on" : "off");
break;
case 'r':
debugRC ^= 1;
print("RC messages %s\r\n", debugRC ? "on" : "off");
break;
case 'R':
print("rebooting...\r\n");
Delay_ms(1000);
reboot();
break;
case 's':
debugSense ^= 1;
print("Sensor messages %s\r\n", debugSense ? "on" : "off");
break;
case 'u':
{
extern int bDeviceState;
printUSART("\r\nYY bDeviceState %3d VCPConnectMode %d\r\n", bDeviceState, GetVCPConnectMode());
break;
}
case 'v':
print("Version: %s\r\n", __EV_VERSION);
break;
case '+':
testPhase += 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
case '-':
testPhase -= 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
/*
case '?':
print("CLI documentation\r\n");
// print("\t'+' test phase output increase (now %5.1f)\r\n", testPhase);
//print("\t'-' test phase output decrease (now %5.1f)\r\n", testPhase);
print("\t'a' autopan messages display (now %s)\r\n", debugAutoPan ? "on" : "off");
print("\t'b' reboot into bootloader\r\n");
print("\t'c' counter messages display (now %s)\r\n", debugCnt ? "on" : "off");
print("\t'd' debug messages display (now %s)\r\n", debugPrint ? "on" : "off");
print("\t'g' dump configuration (binary)\r\n");
print("\t'G' dump configuration (hexadecimal)\r\n");
// print("\t'h' write and save config array\r\n");
print("\t'i' enter config mode (now %s)\r\n", ConfigMode ? "on" : "off");
print("\t'j' leave config mode (now %s)\r\n", ConfigMode ? "on" : "off");
print("\t'o' orientation messages display (now %s)\r\n", debugOrient ? "on" : "off");
print("\t'p' performance messages display (now %s)\r\n", debugPerf ? "on" : "off");
print("\t'r' RC messages display (now %s)\r\n", debugRC ? "on" : "off");
print("\t'R' reboot\r\n");
print("\t's' toggle sensor messages display (now %s)\r\n", debugSense ? "on" : "off");
print("\t'u' print USB state (bDeviceState %3d VCPConnectMode %d)\r\n", bDeviceState, GetVCPConnectMode());
print("\t'v' print version (%s)\r\n", __EV_VERSION);
break;
*/
default:
// TODO
break;
}
}
}
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);
}
void CommHandler(void) //UART4 Interrupt handler implementation
{
int c = GetChar();
if (c >= 0)
{
//ConfigMode = 1;
LEDon();
//print("got char %02X\r\n", c);
switch (c)
{
case 'b':
print("rebooting into boot loader ...\r\n");
Delay_ms(1000);
bootloader();
break;
case 'c':
debugCnt ^= 1;
print("counter messages %s\r\n", debugCnt ? "on" : "off");
break;
case 'd':
debugPrint ^= 1;
print("debug messages %s\r\n", debugPrint ? "on" : "off");
break;
case 'g':
Delay_ms(100);
PutChar('x');
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = configData[i];
PutChar(data);
}
break;
case 'G':
printConfig();
break;
#if 0
case 'H':
if (CharAvailable() >= CONFIGDATASIZE)
{
for (int i = 0; i < CONFIGDATASIZE; i++)
{
uint8_t data = GetChar();
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
}
else
{
UnGetChar(c); // try again in next loop
}
break;
#endif
case 'h':
for (int i = 0; i < CONFIGDATASIZE; i++)
{
int data;
while ((data = GetChar()) < 0)
;
if (data <= LARGEST_CONFIGDATA)
{
configData[i] = data;
}
}
configSave();
break;
case 'i':
ConfigMode = 1;
break;
case 'j':
ConfigMode = 0;
break;
case 'o':
debugOrient ^= 1;
print("Orientation messages %s\r\n", debugOrient ? "on" : "off");
break;
case 'p':
debugPerf ^= 1;
print("performance messages %s\r\n", debugPerf ? "on" : "off");
break;
case 'r':
debugRC ^= 1;
print("RC messages %s\r\n", debugRC ? "on" : "off");
break;
case 'R':
print("rebooting...\r\n");
Delay_ms(1000);
reboot();
break;
case 's':
debugSense ^= 1;
print("Sensor messages %s\r\n", debugSense ? "on" : "off");
break;
case 'u':
{
extern int bDeviceState;
printUSART("\r\nYY bDeviceState %3d VCPConnectMode %d\r\n", bDeviceState, GetVCPConnectMode());
break;
}
case '+':
testPhase += 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
case '-':
testPhase -= 0.1;
print("test phase output %5.1f\r\n", testPhase);
break;
default:
// TODO
break;
}
}
}
//////////////////////////////////////////////////
//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;
}
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);
}
}
