int main(void)
{
DAVE_STATUS_t status;
const uint32_t Button_NotPressed = 1;
uint32_t Button1OldValue = Button_NotPressed;
status = DAVE_Init(); /* Initialization of DAVE APPs */
if (status == DAVE_STATUS_FAILURE)
{
/* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */
XMC_DEBUG("DAVE APPs initialization failed\n");
while (1U)
{
}
}
/* Placeholder for user application code. The while loop below can be replaced with user application code. */
while (1U)
{
if (enable_flooding == ACTIVATED)
{
DIGITAL_IO_SetOutputHigh(&LED2);
CAN_NODE_MO_Transmit(&CAN_NODE_0_LMO_03_Config);
}
else
{
DIGITAL_IO_SetOutputLow(&LED2);
}
{
uint32_t Button1Value = DIGITAL_IO_GetInput(&Button1);
/* react on edge */
if ((Button1Value != Button_NotPressed)
&& (Button1Value != Button1OldValue))
{
if (enable_flooding == ACTIVATED)
{
enable_flooding = NOT_ACTIVATED;
}
else
{
enable_flooding = ACTIVATED;
}
}
Button1OldValue = Button1Value;
}
}
}
int main(void)
{
DAVE_Init(); /* initialize Dave internals */
osKernelInitialize ();
//IO004_EnableOutputDriver(&IO004_Handle0, IO004_OPENDRAIN);
/* Create Task to call Lua_task */
MainThreadId = osThreadCreate(osThread(LUA001_Task), NULL);
osKernelStart();
osDelay(osWaitForever);
for (;;);
return 0;
}
int main(void)
{
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
DAVE_Init(); // Initialization of DAVE Apps
while(1)
{
for(int i=0;i<1000;i++)
for(int j=0;j<10000;j++);
}
return 0;
}
int main(void)
{
DAVE_STATUS_t status; // Startup error
status = DAVE_Init(); // Initialization of DAVE APPs
if(status == DAVE_STATUS_FAILURE)
{
XMC_DEBUG("DAVE APPs initialization failed\n");
while(1U)
{
DIGITAL_IO_SetOutputHigh(&DIGITAL_IO_1); // Turn on LED2 for errors
}
}
startCommRX(); // Start serial connection with the node
while(1U)
{
if(mbTimeout > MB_TIMEOUT) // Check if the current slave is taking too long to reply
// If it is, move on to the next one
{
if(mbCurrentSlave == MB_MAX_SLAVE)
{
mbCurrentSlave = 1;
}
else
{
mbCurrentSlave++;
}
mbReadyForRead = 1;
}
if(mbReadyForRead)
{
mbReadFloat(mbCurrentSlave); // Poll the mbReadyForRead flag and start a new reading
}
if(commReadyForWrite && !queueIsEmpty)
{
commSendData(); // commReadyForWrite - UART Hardware ready
// queueIsEmpty - Readings Queue
}
}
}
int main(void)
{
/*
* Wiring Initialization
*/
wiring_digital_init();
wiring_analog_init();
wiring_time_init();
DAVE_Init();
// Arduino's main() function just calls setup() and loop()....
setup();
while (1) {
loop();
//yield();
}
}
int main(void)
{
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
DAVE_Init(); // Initialization of DAVE Apps
initLSM9DS1();
calibrate(TRUE);
startMeasurements();
while(1)
{
readAndSendMeasurements(NULL);
}
return 0;
}
/**
* \brief This function initializes the hardware
*
*
* \details Sensors are set up, polynomials are calculated and other general preparations are made
*/
void setup(void)
{
initBluetoothStorage();//initialize space for variables used for Bluetooth Communication
//-------------------Dave Setup---------------------
DAVE_STATUS_t status;
status = DAVE_Init();//Initialization of DAVE APPs
if (status == DAVE_STATUS_FAILURE)
{
/* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */
XMC_DEBUG("DAVE APPs initialization failed\n");
while (1U);
}
disableIRQ();//disables all Interrupts
delay(2000);//waits 2000ms
enableIRQ();//enables configurated Interrupts
setup_STATE_LEDs();//setup Status-LED's
WatchRC_Init();//initialize RC-Watchdog
setup_UART_Trigger_limits();//setup Trigger-Limits of UART-FIFO
PressureFIR = Initialize_FIR_Filter(PressureFIR, MOVING_AVERAGE);//initialize FIR Filter
setupDPS310I2C();//initialize DPS310
getCoefficients();//get Coefficients of DPS310
setupDPS310();//setup DPS Hardware
MPU9150_Setup();//configures the IMU
delay(3000);//wait 3000ms to wait for ESC's to startup
// initialize controller polynomials
polynoms.b_roll[0]=parameter.P_roll-parameter.I_roll*parameter.T-parameter.P_roll*parameter.N_roll*parameter.T+parameter.N_roll*parameter.I_roll*parameter.T*parameter.T+parameter.D_roll*parameter.N_roll;
polynoms.b_roll[1]=parameter.I_roll*parameter.T-2*parameter.P_roll+parameter.P_roll*parameter.N_roll*parameter.T-2*parameter.D_roll*parameter.N_roll;
polynoms.b_roll[2]=parameter.P_roll+parameter.D_roll*parameter.N_roll;
polynoms.a_roll[0]=1-parameter.N_roll*parameter.T;
polynoms.a_roll[1]=parameter.N_roll*parameter.T-2;
polynoms.b_pitch[0]=parameter.P_pitch-parameter.I_pitch*parameter.T-parameter.P_pitch*parameter.N_pitch*parameter.T+parameter.N_pitch*parameter.I_pitch*parameter.T*parameter.T+parameter.D_pitch*parameter.N_pitch;
polynoms.b_pitch[1]=parameter.I_pitch*parameter.T-2*parameter.P_pitch+parameter.P_pitch*parameter.N_pitch*parameter.T-2*parameter.D_pitch*parameter.N_pitch;
polynoms.b_pitch[2]=parameter.P_pitch+parameter.D_pitch*parameter.N_pitch;
polynoms.a_pitch[0]=1-parameter.N_pitch*parameter.T;
polynoms.a_pitch[1]=parameter.N_pitch*parameter.T-2;
TIMER_Start(&Control_Timer);//start Timer for Controller
}
int main(void)
{
DAVE_STATUS_t status;
status = DAVE_Init(); /* Initialization of DAVE APPs */
if(status == DAVE_STATUS_FAILURE)
{
/* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */
XMC_DEBUG("DAVE APPs initialization failed\n");
while(1U)
{
}
}
tempinit();
/* Placeholder for user application code. The while loop below can be replaced with user application code. */
while(1U)
{
}
}
int main(void)
{
Control_P0_9(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_2(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_1(OUTPUT_PP_GP, VERYSTRONG);
Control_P3_0(OUTPUT_PP_GP, VERYSTRONG);
Control_P0_2(OUTPUT_PP_GP, VERYSTRONG);
Control_P0_3(OUTPUT_PP_GP, VERYSTRONG);
Control_P2_0(OUTPUT_PP_GP, VERYSTRONG);
Control_P2_7(OUTPUT_PP_GP, VERYSTRONG);
SET_P0_2;
SET_P0_3;
SET_P2_0;
SET_P2_7;
#ifdef LED_test
int ortime=0L;
while(ortime<10){
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_1;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P3_2;
for (int orcount = 0; orcount<200000; orcount++)__NOP();
TOGGLE_P0_9;
ortime++;
}
#endif
DAVE_Init(); // Initialization of DAVE Apps
Initialize();
Control_P0_0(INPUT, STRONG); //SET UART PIN to TRISTATE: necessary with LARIX_V3
Control_P0_1(INPUT, STRONG); //SET UART PIN to TRISTATE: necessary with LARIX_V3
#ifdef DPS310
SensorError err = setupDPS310();
if(err)
ortime=0;
while(ortime<20){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_2;
ortime++;
}
#endif
// /* Initialization of the USBD_VCOM App */
if(USBD_VCOM_Init() != DAVEApp_SUCCESS)
{
while(1){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_1;
ortime++;
}
return -1;
}
#ifdef SERIAL_DEBUG
uint32_t power = 0;
#endif
#ifdef DEBUG_SPECIFIC
#undef DEBUG_CONTINOUS
uint32_t p = 0;
uint32_t t = 0;
#endif
#ifdef DEBUG_CONTINOUS
uint32_t counterccc = 0;
uint32_t helper = 0;
#endif
ortime=0;
while(ortime<20){
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_0;
for (int orcount = 0; orcount<300000; orcount++)__NOP();
TOGGLE_P3_1;
ortime++;
}
while(1)
{
updateValues(&p,&t);
#ifndef SERIAL_DEBUG
if(newvalue){
CalculateActuatorSpeed_Percent(&u_roll, &u_pitch, &u_yaw_dot, &powerD, actuator_speed_percent, &YPR[1], &YPR[2]);
#ifdef PWM_OUTPUT
//Scale percent Output
PWM_width[0]=0.45*actuator_speed_percent[3]+45;
PWM_width[1]=0.45*actuator_speed_percent[2]+45;
PWM_width[2]=0.45*actuator_speed_percent[0]+45;
PWM_width[3]=0.45*actuator_speed_percent[1]+45;
//set actors //normal//LARIX with PWMoutput
#ifdef LARIX_with_PWM_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[3]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[1]);
#endif
#ifdef WIDEFIELD_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
// #else
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
#endif
#endif
#ifdef UART_SC_IF
#ifdef IRMCK
uint32_t data_TX;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[1]=(uint8_t)(data_TX>>8);
DaisyTransmit[2]=(uint8_t) data_TX;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[4]=(uint8_t)(data_TX>>8);
DaisyTransmit[5]=(uint8_t) data_TX;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[7]=(uint8_t)(data_TX>>8);
DaisyTransmit[8]=(uint8_t) data_TX;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[10]=(uint8_t)(data_TX>>8);
DaisyTransmit[11]=(uint8_t) data_TX;
UART001_WriteDataBytes(&ESC_UART_Handle, DaisyTransmit, 12);
#else
uint16_t data;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data=actuator_speed_percent[0]/100.0*0xFFFF;
DaisyTransmit[1]=(uint8_t)(data>>8);
DaisyTransmit[2]=(uint8_t) data;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data=actuator_speed_percent[1]/100.0*0xFFFF;
DaisyTransmit[4]=(uint8_t)(data>>8);
DaisyTransmit[5]=(uint8_t) data;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data=actuator_speed_percent[2]/100.0*0xFFFF;
DaisyTransmit[7]=(uint8_t)(data>>8);
DaisyTransmit[8]=(uint8_t) data;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data=actuator_speed_percent[3]/100.0*0xFFFF;
DaisyTransmit[10]=(uint8_t)(data>>8);
DaisyTransmit[11]=(uint8_t) data;
UART001_WriteDataBytes(&UART001_Handle2, DaisyTransmit, 12);
#endif
#endif
newvalue=0;
}
#else
#ifdef PWM_OUTPUT
//Scale percent Output
actuator_speed_percent[0]=powerD;
actuator_speed_percent[1]=powerD;
actuator_speed_percent[2]=powerD;
actuator_speed_percent[3]=powerD;
PWM_width[0]=0.45*actuator_speed_percent[0]+45;
PWM_width[1]=0.45*actuator_speed_percent[1]+45;
PWM_width[2]=0.45*actuator_speed_percent[2]+45;
PWM_width[3]=0.45*actuator_speed_percent[3]+45;
//set actors //normal//LARIX with PWMoutput
#ifdef LARIX_with_PWM_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[3]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[1]);
#endif
#ifdef WIDEFIELD_used
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
// #else
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle0, PWM_width[0]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle1, PWM_width[1]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle2, PWM_width[2]);
// PWMSP001_SetDutyCycle((PWMSP001_HandleType*)&ESC_PWM_Handle3, PWM_width[3]);
#endif
#endif
#ifdef UART_SC_IF
#ifdef IRMCK
uint32_t data_TX;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[1]=(uint8_t)(data_TX>>8);
DaisyTransmit[2]=(uint8_t) data_TX;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[4]=(uint8_t)(data_TX>>8);
DaisyTransmit[5]=(uint8_t) data_TX;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[7]=(uint8_t)(data_TX>>8);
DaisyTransmit[8]=(uint8_t) data_TX;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data_TX=(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0);
if(powerD<10) data_TX = 0;
DaisyTransmit[10]=(uint8_t)(data_TX>>8);
DaisyTransmit[11]=(uint8_t) data_TX;
UART001_WriteDataBytes(&ESC_UART_Handle, DaisyTransmit, 12);
#else
uint16_t data;
//Motor 1
DaisyTransmit[0]=SET_MOTOR_SPEED;
data=actuator_speed_percent[0]/100.0*0xFFFF;
DaisyTransmit[1]=(uint8_t)(data>>8);
DaisyTransmit[2]=(uint8_t) data;
//Motor 2
DaisyTransmit[3]=SET_MOTOR_SPEED;
data=actuator_speed_percent[1]/100.0*0xFFFF;
DaisyTransmit[4]=(uint8_t)(data>>8);
DaisyTransmit[5]=(uint8_t) data;
//Motor 3
DaisyTransmit[6]=SET_MOTOR_SPEED;
data=actuator_speed_percent[2]/100.0*0xFFFF;
DaisyTransmit[7]=(uint8_t)(data>>8);
DaisyTransmit[8]=(uint8_t) data;
//Motor 4
DaisyTransmit[9]=SET_MOTOR_SPEED;
data=actuator_speed_percent[3]/100.0*0xFFFF;
DaisyTransmit[10]=(uint8_t)(data>>8);
DaisyTransmit[11]=(uint8_t) data;
UART001_WriteDataBytes(&UART001_Handle2, DaisyTransmit, 12);
#endif
#endif
#endif
#ifdef DEBUG_SPECIFIC
#undef DEBUG_CONTINOUS
// Check number of bytes received
Bytes = USBD_VCOM_BytesReceived();
if(Bytes != 0)
{
for(nByte = 0; nByte < Bytes; nByte++)
{
// Receive Byte
if(USBD_VCOM_ReceiveByte(&USB_Rx_Buffer[0]) != DAVEApp_SUCCESS)
{
//Error
}
}
switch(USB_Rx_Buffer[0])
{
case '1':
sprintf(USB_Tx_Buffer, "Throttle: %f Rudder: %f Elevator: %f Aileron: %f\n", powerD, yawD_dot, pitchD, rollD);
break;
case '2':
PWMSP001_Start(&MagCalib_Timer);
break;
case '3':
sprintf(USB_Tx_Buffer, "Y:%1.2f P:%1.2f R:%1.2f YOff:%1.2f\n", YPR[0], YPR[1], YPR[2], yoffset);
break;
case '4':
sprintf(USB_Tx_Buffer, "Y_dot:%1.2f\n", yaw_dot);
break;
case '5':
sprintf(USB_Tx_Buffer, "PWM1:%f PWM2:%f PWM3:%f PWM4:%f\n", PWM_width[0], PWM_width[1], PWM_width[2], PWM_width[3]);
break;
case '6':
sprintf(USB_Tx_Buffer, "PWM1:%f PWM2:%f PWM3:%f PWM4:%f\n", actuator_speed_percent[0], actuator_speed_percent[1], actuator_speed_percent[2], actuator_speed_percent[3]);
break;
case '7':
sprintf(USB_Tx_Buffer, "eY:%f eP:%f eR:%f\n", yawD_dot-yaw_dot, pitchD-YPR[1], rollD-YPR[2]);
break;
case '8':
sprintf(USB_Tx_Buffer, "TimerSensor:%d TimerMain:%d TimerRC:%d\n", (int)GetSensorCount(), (int)counter_main, (int)GetRCCount());
break;
case '9':
updateValues(&p,&t);
sprintf(USB_Tx_Buffer, "Pressure: %d Temperature: %d Counter: %d\n",(int)p,(int)t,(int)DPS310_INT_counter);
break;
case 'a':
#ifdef SERIAL_DEBUG
power += 10;
if (power > 100) power = 100;
powerD = power;
sprintf(USB_Tx_Buffer, "Set Speed to: %d\n",(int)power);
#endif
break;
case 'b':
#ifdef SERIAL_DEBUG
power = 0;
powerD = power;
sprintf(USB_Tx_Buffer, "Set Speed to: %d\n",(int)power);
#endif
break;
default:
sprintf(USB_Tx_Buffer, "Unknown Command\n");
break;
}
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
}
if (sendMag)
{
sendMag = FALSE;
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
}
//Call continuous to handle class specific control request
/* Main USB management task */
/* Check if data received */
CDC_Device_USBTask(&USBD_VCOM_CDCInterface);
#endif
#ifdef DEBUG_CONTINOUS
counterccc++;
if(counterccc % 500 == 0)
{
helper++;
// sprintf(USB_Tx_Buffer, "{PacketC,T,%d}\n",packets);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Loss,T,%d}\n",packet_loss);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{UndefE,T,%d}\n",undef_error);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Timeout,T,%d}\n",timeout_count);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Throttle,T,%.3f}\n",powerD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Rudder,T,%.3f}\n",yawD_dot);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Elevator,T,%.3f}\n",pitchD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
// sprintf(USB_Tx_Buffer, "{Aileron,T,%.3f}\n",rollD);
// USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor1,T,%u}\n",(uint16_t)((actuator_speed_percent[0]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor2,T,%u}\n",(uint16_t)((actuator_speed_percent[1]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor3,T,%u}\n",(uint16_t)((actuator_speed_percent[2]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
sprintf(USB_Tx_Buffer, "{Motor4,T,%u}\n",(uint16_t)((actuator_speed_percent[3]/100.0)*65535.0));
USBD_VCOM_SendString((const char *)USB_Tx_Buffer);
counterccc=0;
}
//Call continuous to handle class specific control request
/* Main USB management task */
/* Check if data received */
CDC_Device_USBTask(&USBD_VCOM_CDCInterface);
#endif
}
return 0;
}
int main(void)
{
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
DAVE_Init(); // Initialization of DAVE Apps
int counter = 0;
handle_t TimerId;
uint32_t Status = SYSTM001_ERROR;
addressAndData adrAndData;
adrAndData.adr.addressDevice[0] = 0x6B;
adrAndData.adr.addressDevice[1] = 0x1E;
adrAndData.adr.subAddress[0] = OUT_X_L_XL; //subaddres for accel
adrAndData.adr.subAddress[1] = OUT_X_L_G; //sub address for gyroscope
adrAndData.adr.subAddress[2] = OUT_X_L_M;
initLSM9DS1();
calibrate(TRUE);
//readAccel1();
//makeTimer(100, SYSTM001_PERIODIC, timerHandlerReadByte1, &a, &Status, &TimerId);
TimerId=SYSTM001_CreateTimer(2,SYSTM001_PERIODIC,timerHandlerReadByte1,&adrAndData);
SYSTM001_StartTimer(TimerId);
while(1)
{
if(!readingAllowed)
{
int16_t accelX = (adrAndData.dane[1] << 8) | adrAndData.dane[0]; // Store x-axis values into gx
int16_t accelY = (adrAndData.dane[3] << 8) | adrAndData.dane[2]; // Store y-axis values into gy
int16_t accelZ = (adrAndData.dane[5] << 8) | adrAndData.dane[4]; // Store z-axis values into gz
if (_autoCalc) //kalibracja
{
accelX -= aBiasRaw[X_AXIS];
accelX -= aBiasRaw[Y_AXIS];
accelX -= aBiasRaw[Z_AXIS];
}
accelX = calcAccel(accelX);
accelY = calcAccel(accelY);
accelZ = calcAccel(accelZ);
pomiary[counter].ax = accelX;
pomiary[counter].ay = accelY;
pomiary[counter].az = accelZ;
int16_t gyroX = (adrAndData.dane[7] << 1) | adrAndData.dane[6];
int16_t gyroY = (adrAndData.dane[9] << 1) | adrAndData.dane[8];
int16_t gyroZ = (adrAndData.dane[11] << 1) | adrAndData.dane[10];
if (_autoCalc) //kalibracja
{
gyroX -= gBiasRaw[X_AXIS];
gyroY -= gBiasRaw[Y_AXIS];
gyroZ -= gBiasRaw[Z_AXIS];
}
gyroX = calcGyro(gyroX);
gyroY = calcGyro(gyroY);
gyroZ = calcGyro(gyroZ);
pomiary1[counter].ax = gyroX;
pomiary1[counter].ay = gyroY;
pomiary1[counter].az = gyroZ;
counter++;
readingAllowed = TRUE;
}
if(counter >= 100)
{
counter = 0;
}
}
return 0;
}
int main(void) {
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
PORT0 ->HWSEL &= ~0x0000c000UL; //Faz pin 0.7 funcionar
PORT0 ->HWSEL |= 0 << 14;
DAVE_Init(); // Initialization of DAVE Apps
//PORT0->HWSEL &= ~0x0000c000UL; //Faz pin 0.7 funcionar
//PORT0->HWSEL |= 0 << 14;
/*Etapa de inicializacao*/
configure_E(); //Configura transceptor como emissor
//IO004_SetPin(LED1); //Leds para debug
//IO004_SetPin(LED2);
//VER COMOFAS pra ligar analog do controle aqui ja
psxHandShake();
psxConfiguraControle();
ADC001_GenerateLoadEvent(&ADC001_Handle0);
ADC001_GetResult(&ADC001_Handle0, &result);
Software_Timers_Init();
/*Loop do controle*/
while (1) {
ADC001_GetResult(&ADC001_Handle0, &result);
pwm_max = PWM_LIM;
/*Inicializa o que sera mandado*/
BOOLType blah2 = 1;
BOOLType buzina = 1;
BOOLType enable = 0;
BOOLType albh2 = 1;
BOOLType blah1 = 1;
BOOLType albh1 = 1;
int16_t pow1, pow2;
/*Le controle*/
psxLeControle();
if (psx_status != 140) //Nao ta analogico
{
psxHandShake();
psxConfiguraControle();
continue;
}
/*Com dados do controle atribui valores e chama callbacks*/
if (START && start) {
start_state = 1;
start();
}
if (SELECT && select) {
select_state = 1;
select();
}
if (L_DOIS && l_dois) {
l_dois_state = 1;
l_dois();
}
if (L_UM && l_um) {
l_um_state = 1;
l_um();
}else if(!R_UM){
acceleration_ticks=0;
}
if (L_TRES && l_tres) {
l_tres_state = 1;
l_tres();
}
if (R_UM && r_um) {
r_um_state = 1;
r_um();
}else if(!L_UM){
acceleration_ticks = 0;
}
if (R_DOIS && r_dois) {
r_dois_state = 1;
r_dois();
}
if (R_TRES && r_tres) {
r_tres_state = 1;
r_tres();
}
if (SQR && sqr) {
sqr_state = 1;
sqr(DEGRAU);
// changeMode(DEGRAU);
}
if (TRIANGLE && triangle) {
triangle_state = 1;
triangle(LINEAR);
// changeMode(LINEAR);
}
if (CIRCLE && circle) {
circle_state = 1;
circle(EXP);
// changeMode(EXP);
}
if (CROSS && cross) {
cross_state = 1;
cross();
}
if (LEFT && left) {
left_state = 1;
left();
}
if (RIGHT && right) {
right_state = 1;
right();
}
if (UP && up) {
up_state = 1;
up();
}
if (DOWN && down) {
down_state = 1;
down();
}
//
data_E[0] = pwm_max;
//if (psxDado[5] == 0 && psxDado[3] == 0) continue; //Enquanto for zero nao faz nada -> tirar quando ligar o analogico
pow1 = (psxDado[5] - 127); //<<1; //Analog esq //Subtrai 127 para saber o sentido
pow2 = (psxDado[3] - 127); //<<1;
data_E[3] = 0;
int16_t temp; //Variavel para armazenamento temporario dos calculos
if (!flipped) //Robo virado, variavel atribuida pelo clique de um botao
{
if (pow1 < -30)
albh2 = 0; //ok
else if (pow1 > 30)
blah2 = 0; //ok
else {
albh2 = 1;
blah2 = 0;
}
if (pow2 < -30)
albh1 = 0;
else if (pow2 > 30)
blah1 = 0;
else {
albh1 = 1;
blah1 = 0;
}
temp = pow1 > 0 ? pow1 * 2 : (-pow1) * 2;
if (temp > 255)
temp = 255;
data_E[1] = temp * pwm_max / 100;
temp = pow2 > 0 ? pow2 * 2 : (-pow2) * 2;
if (temp > 255)
temp = 255;
data_E[2] = temp * pwm_max / 100;
} else {
if (pow2 > 30)
albh2 = 0; //ok
else if (pow2 < -30)
blah2 = 0; //ok
else {
albh2 = 1;
blah2 = 0;
}
if (pow1 > 30)
albh1 = 0;
else if (pow1 < -30)
blah1 = 0;
else {
albh1 = 1;
blah1 = 0;
}
temp = pow1 > 0 ? pow1 * 2 : (-pow1) * 2;
if (temp > 255)
temp = 255;
data_E[2] = temp * pwm_max / 100;
temp = pow2 > 0 ? pow2 * 2 : (-pow2) * 2;
if (temp > 255)
temp = 255;
data_E[1] = temp * pwm_max / 100;
}
//if (data_E[1] > 20 || data_E[2] > 20) enable = 1;
char data_0 = (mode & 1);
char data_1 = ((mode >> 1) & 1);
// data_E[3] = data_E[3] | (blah1 << BLAH1) | (blah2 << BLAH2)
// | (albh1 << ALBH1) | (albh2 << ALBH2) | (enable << ENABLE)
// | (buzina << BUZINA);
data_E[3] = data_E[3] | (blah1 << BLAH1) | (blah2 << BLAH2)
| (albh1 << ALBH1) | (albh2 << ALBH2) | (data_1 << ENABLE)
| (data_0 << BUZINA);
data_E[4] = result.Result >> 4; //Resultado tem precisao de 12bits, divide por 16 para obter 8 bits = 1 byte
last_value_left = data_E[1];
last_value_right = data_E[2];
write_E();
updateButtonStates();
}
return 0;
}
