/*---------------------------------------------------------------------------*
* Routine: MainMenu
*---------------------------------------------------------------------------*
* Description:
* Present the main menu
*---------------------------------------------------------------------------*/
void MainMenu(void)
{
T_uezDevice lcd;
T_pixelColor *pixels;
// Open the LCD and get the pixel buffer
if (UEZLCDOpen("LCD", &lcd) == UEZ_ERROR_NONE) {
UEZLCDGetFrame(lcd, 0, (void **)&pixels);
#if (!FAsT_STARTUP)
// Clear the screen
TitleScreen();
//while(1);
PlayAudio(523, 100);
PlayAudio(659, 100);
PlayAudio(783, 100);
PlayAudio(1046, 100);
PlayAudio(783, 100);
PlayAudio(659, 100);
PlayAudio(523, 100);
// Force calibration?
Calibrate(CalibrateTestIfTouchscreenHeld());
UEZTaskDelay(1000);
#else
UEZLCDBacklight(lcd, 255);
Calibrate(FALSE);
#endif
AppMenu(&mainmenu);
UEZLCDClose(lcd);
}
}
void MainWindow::keyPressEvent(QKeyEvent *e)
{
switch( e->key() )
{
case Qt::Key_Escape:
close();
break;
case Qt::Key_C:
emit(Calibrate());
break;
case Qt::Key_D:
emit(Detect());
break;
case Qt::Key_R:
if(this->isMaximized())
resize(800, 600);
else
this->showMaximized();
break;
case Qt::Key_T:
this->Toggle();
break;
case Qt::Key_1:
emit(DiceRegisterSetup());
break;
case Qt::Key_2:
emit(DiceRegisterMain());
break;
}
}
bool FloppyDrive::initialize()
{
std::stringstream devstring;
devstring << "/dayos/dev/fdc" << m_devno;
int retval = vfs_create_device(devstring.str().c_str(), VFS_MODE_RW, VFS_BLOCK_DEVICE);
if (retval == SIGNAL_FAIL)
{
debug_printf("[ FDC ] Could not create device file!\n");
return false;
}
InitDMA();
Reset();
int result = VersionCommand();
if(result == ERR_NOT_SUPPORTED)
{
debug_printf("[ FDC ] Your drive is not supported!\n");
return false;
}
if(ConfigureAndLock() == ERR_TIMEOUT)
{
debug_printf("[ FDC ] Timeout while configuring drive!\n");
return false;
}
Calibrate(m_devno);
// Hard code for 1.44M floppies
setBlockCount(2880);
debug_printf("[ FDC ] Initialized drive %d\n", m_devno);
return true;
}
void CPMotion::OnCalibrate()
{
//sets robot at 0,0 pointing north (90deg)
double Heading = pi / 2;
long X = 0;
long Y = 0;
Calibrate(Heading, X, Y);
}
void Phantom::Run() {
hdStartScheduler();
if (HD_DEVICE_ERROR(_error = hdGetError()))
{
cout << "Failed to start the scheduler" << endl;//, &error);
return;
}
Calibrate();
}
/**
* Gyro constructor with a precreated AnalogInput object.
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
: m_analog(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitGyro();
Calibrate();
}
}
double Estimate3D(std::vector<Wml::Vector2d> &points_2d,
std::vector<Wml::Matrix4d> &cameras,
std::vector<Wml::Matrix3d> &Ks,
Wml::Vector3d &initx)
{
ZOptimizerLM<double> optimizer;
ZReprojective_LinearExpress express;
ZReprojective_NonLinearExpress express2;
Wml::GVectord x(3);
double MinValue=0;
int iPtCount = points_2d.size();
express.PList.resize(iPtCount);
express.m_points.resize(iPtCount);
express.wList.resize(iPtCount);
express2.PList.resize(iPtCount);
express2.m_points.resize(iPtCount);
express2.wList.resize(iPtCount);
for(int i = 0; i < iPtCount; ++ i)
{
double x = points_2d[i].X();
double y = points_2d[i].Y();
express.PList[i] = cameras[i];
Calibrate(x, y, Ks[i]);
express.m_points[i].X() = x;
express.m_points[i].Y() = y;
express.wList[i] = (Ks[i][0][0] + Ks[i][1][1]) / 2.0;
express2.PList[i] = express.PList[i];
express2.m_points[i] = express.m_points[i];
express2.wList[i] = express.wList[i];
}
x[0] = initx[0];
x[1] = initx[1];
x[2] = initx[2];
optimizer.Optimize(express,x,MinValue);
optimizer.Optimize(express2,x,MinValue);
initx[0] = x[0];
initx[1] = x[1];
initx[2] = x[2];
double RMSE = Track_RMSE(points_2d, cameras, Ks, initx);
return RMSE;
}
/*---------------------------------------------------------------------------*
* Routine: MainMenu
*---------------------------------------------------------------------------*
* Description:
* Present the main menu
*---------------------------------------------------------------------------*/
void MainMenu(void)
{
T_uezDevice lcd;
T_pixelColor *pixels;
// Open the LCD and get the pixel buffer
if (UEZLCDOpen("LCD", &lcd) == UEZ_ERROR_NONE) {
UEZLCDGetFrame(lcd, 0, (void **)&pixels);
#if (!FAsT_STARTUP)
// Clear the screen
TitleScreen();
//while(1);
PlayAudio(523, 100);
PlayAudio(659, 100);
PlayAudio(783, 100);
PlayAudio(1046, 100);
PlayAudio(783, 100);
PlayAudio(659, 100);
PlayAudio(523, 100);
// Force calibration?
Calibrate(CalibrateTestIfTouchscreenHeld());
UEZTaskDelay(1000);
#else
UEZLCDBacklight(lcd, 255);
Calibrate(FALSE);
#endif
// Set the screen saver icon
BouncingLogoSS_Setup(
(TUInt8 *)G_uEZLogo,
UEZ_ICON_WIDTH,
UEZ_ICON_HEIGHT,
DISPLAY_WIDTH,
DISPLAY_HEIGHT);
AppMenu(&mainmenu);
UEZLCDClose(lcd);
}
}
void Loop(int sock)
{
XnStatus nRetVal = XN_STATUS_OK;
struct timespec last,now;
double nowtime, starttime;
clock_gettime(CLOCK_REALTIME, &last);
double lasttime = (double)(last.tv_sec) + ((double)(last.tv_nsec))/1000000000;
int frames=0;
while (g_notDone)
{
if ((nRetVal = g_context.WaitAndUpdateAll()) != XN_STATUS_OK)
{
fprintf(stderr,"Could not update ir: %s\n", xnGetStatusString(nRetVal));
continue;
}
const XnDepthPixel* pDepthMap = g_depth.GetDepthMap();
const XnRGB24Pixel* pImage = NULL;//g_image.GetRGB24ImageMap();
const XnIRPixel* pIR = NULL;//g_ir.GetIRMap();
ProcessDepthFrame(pDepthMap, g_depthWidth, g_depthHeight);
FindFingertip();
frames++;
clock_gettime(CLOCK_REALTIME, &now);
nowtime = (double)(now.tv_sec) + ((double)(now.tv_nsec))/1000000000;
if (g_stabalize) // If we are still stablizing then don't do anything
{
if ((nowtime - starttime) >= STABILIZATION_TIME_SECS)
{
g_stabalize = FALSE;
g_set_background = TRUE;
}
}
else if (g_calibrate) // Do we need to calibrate?
Calibrate(sock);
else
SendFingers(sock); // otherwise just send the touches
/*
if (nowtime - lasttime >= 1.0)
{
printf("%d FPS\n", (int)(frames/(nowtime-lasttime)));
lasttime = nowtime;
frames = 0;
if (sock >= 0 && pDepthMap != 0 )// && pImage != 0 )//&& pIR != 0)
SendImage(sock,pDepthMap, pImage, pIR, g_depthWidth, g_depthHeight);
}
*/
}
}
void IMU::Loop( float dt )
{
if ( mState == Off ) {
// Nothing to do
} else if ( mState == Calibrating or mState == CalibratingAll ) {
Calibrate( dt, ( mState == CalibratingAll ) );
} else if ( mState == CalibrationDone ) {
mState = Running;
} else if ( mState == Running ) {
UpdateRPY( dt );
}
}
/**
* @brief Read Acc values, Calibrate them, filter them, and update the pitch value to be displayed
* @param None
* @retval None
*/
void ReadAcc(void){
/* Get values */
LIS3DSH_ReadACC(accValue);
Calibrate(accValue);
/* Filter values */
kalmanUpdate(xState, accValue[0]);
kalmanUpdate(yState, accValue[1]);
kalmanUpdate(zState, accValue[2]);
pitchAngle = calcPitch(xState->x, yState->x, zState->x);
rollAngle = calcRoll(xState->x, yState->x, zState->x);
}
static ERROR_T DataUpdate(IvhSensor* me, const IvhSensorData* data, const IvhSensorType type)
{
ERROR_T retVal = ERROR_OK;
pIvhSensorOrientation sensor = (pIvhSensorOrientation)(me);
const int16_t MIN_ACCEL_CHANGE = 10;
switch(type)
{
case IVH_SENSOR_TYPE_ACCELEROMETER3D :
// The normal minimum sensitivity for accel is 35mg. We can set it
// to zero and get all samples, but it's too jittery. This is a secondary
// filter that works as if we specified a sensitivity of 10mg
if (fabs(sensor->lastCalibratedAccel[AXIS_X] - data->data.accel.xyz.x) > MIN_ACCEL_CHANGE ||
fabs(sensor->lastCalibratedAccel[AXIS_Y] - data->data.accel.xyz.y) > MIN_ACCEL_CHANGE ||
fabs(sensor->lastCalibratedAccel[AXIS_Z] - data->data.accel.xyz.z) > MIN_ACCEL_CHANGE)
{
sensor->lastCalibratedAccel[AXIS_X] = (ROTATION_VECTOR_T)(data->data.accel.xyz.x);
sensor->lastCalibratedAccel[AXIS_Y] = (ROTATION_VECTOR_T)(data->data.accel.xyz.y);
sensor->lastCalibratedAccel[AXIS_Z] = (ROTATION_VECTOR_T)(data->data.accel.xyz.z);
}
sensor->lastTimestampAccel = data->timeStampInMs;
break;
case IVH_SENSOR_TYPE_GYROSCOPE3D :
sensor->lastCalibratedGyro[AXIS_X] = (ROTATION_VECTOR_T)(data->data.gyro.xyz.x);
sensor->lastCalibratedGyro[AXIS_Y] = (ROTATION_VECTOR_T)(data->data.gyro.xyz.y);
sensor->lastCalibratedGyro[AXIS_Z] = (ROTATION_VECTOR_T)(data->data.gyro.xyz.z);
sensor->lastTimestampGyro = data->timeStampInMs;
Calibrate(me);
break;
case IVH_SENSOR_TYPE_MAGNETOMETER3D :
sensor->lastCalibratedMag[AXIS_X] = data->data.mag.xyzCalibrated.x;
sensor->lastCalibratedMag[AXIS_Y] = data->data.mag.xyzCalibrated.y;
sensor->lastCalibratedMag[AXIS_Z] = data->data.mag.xyzCalibrated.z;
sensor->lastRotatedMag[AXIS_X] = data->data.mag.xyzRotated.x;
sensor->lastRotatedMag[AXIS_Y] = data->data.mag.xyzRotated.y;
sensor->lastRotatedMag[AXIS_Z] = data->data.mag.xyzRotated.z;
sensor->lastRawMag[AXIS_X] = (ROTATION_VECTOR_T)(data->data.mag.xyzRaw.x);
sensor->lastRawMag[AXIS_Y] = (ROTATION_VECTOR_T)(data->data.mag.xyzRaw.y);
sensor->lastRawMag[AXIS_Z] = (ROTATION_VECTOR_T)(data->data.mag.xyzRaw.z);
memcpy(&sensor->lastMagCovariance,
data->data.mag.covariance,
(COVARIANCE_MATRIX_SIZE * sizeof(float)));
sensor->lastTimestampMag = data->timeStampInMs;
break;
default:
TraceLog(TRACE_LEVEL_ERROR, TRACE_ALGO, "[%!FUNC!]invalid sensor type, expect [IVH_SENSOR_TYPE_ACCELEROMETER3D|IVH_SENSOR_TYPE_GYROSCOPE3D|IVH_SENSOR_TYPE_MAGNETOMETER3D]");
}
return retVal;
}
static void calculate_compass()
{
int magX = 0; int magY = 0; int magZ = 0;
static int count = 0;
int iResult;
int offsets[3] = {0};
double dHeading = 0;
float accForward;
float accLeft;
orietation_data[0] = gsensor_data[0];
orietation_data[1] = gsensor_data[1];
orietation_data[2] = gsensor_data[2];
compass_data[0] = g_compass_data.y;
compass_data[1] = -g_compass_data.x;
compass_data[2] = g_compass_data.z;
magX = compass_data[0];
magY = compass_data[1];
magZ = compass_data[2];
count++;
if (count < 1200){
CollectDataItem(magX, magY, magZ);
//LOGE("CollectData.\n");
}
iResult = Calibrate(&offsets[0]);
// Elsewhere in the application, getting magnetic data and correcting out the
// hard iron disturbances
magX -= offsets[0];
magY -= offsets[1];
magZ -= offsets[2];
//Magnetic field data is now ready to be used
accForward = 0;
accLeft = 0;
dHeading = Heading(-magX,magY,magZ,accForward ,accLeft);
if (dHeading >0)
{
// Use the Heading Value
g_compass_data.x = dHeading;
}
Calculate_Sensor_Angle();
g_compass_data.y = sensor_angle[1];
g_compass_data.z = sensor_angle[2];
}
void CTravelTab::OnLocate()
{
Main->bMemoryAccess = AdjustPrivileges();
DWORD dwProcessID;
HANDLE m_hProcess;
LPVOID lpAddress;
CString csCoords;
byte * pointer = new byte[Main->sizeX];
byte * pointer1 = new byte[Main->sizeY];
byte * pointer2 = new byte[Main->sizeZ];
byte * pointer3 = new byte[Main->sizeM];
if (!Main->calibrated)
Calibrate();
if (Main->calibrated)
{
EnumWindows(EnumWindowsProc, 1);
if ((Main->bMemoryAccess) && (hwndUOClient))
{
GetWindowThreadProcessId(hwndUOClient, &dwProcessID);
m_hProcess = OpenProcess(PROCESS_ALL_ACCESS, FALSE, dwProcessID);
lpAddress = (LPVOID)((DWORD_PTR)Main->LocX);
ReadProcessMemory(m_hProcess, lpAddress, pointer, Main->sizeX, NULL);
int x = (short)(pointer[0] | (pointer[1] << 8));
lpAddress = (LPVOID)((DWORD_PTR)Main->LocY);
ReadProcessMemory(m_hProcess, lpAddress, pointer1, Main->sizeY, NULL);
int y = (short)(pointer1[0] | (pointer1[1] << 8));
lpAddress = (LPVOID)((DWORD_PTR)Main->LocZ);
ReadProcessMemory(m_hProcess, lpAddress, pointer2, Main->sizeZ, NULL);
int z = (short)(pointer2[0] | (pointer2[1] << 8));
lpAddress = (LPVOID)((DWORD_PTR)Main->LocM);
ReadProcessMemory(m_hProcess, lpAddress, pointer3, Main->sizeM, NULL);
int m = (byte)pointer3[0];
csCoords.Format("%d",m);
m_ceMapPlane.SetWindowText(csCoords);
Recenter((short)x,(short)y,(short)z);
CloseHandle( m_hProcess );
}
else
Main->calibrated = false;
}
}
/*
* Reset and calibrate the controller.
* Return true is successful, false otherwise.
*/
static bool Reset_Controller(void)
{
/* Reset */
Out_Byte(FDC_DOR_REG, 0);
/*Micro_Delay(1000); */
/*
* Enable fd0
* TODO: we might want to support drives other than 0 eventually
*/
Start_Motor(0);
return Calibrate(0);
}
bool
NoOpLoopUnrolled::AllSet()
{
/* check to see if we need calibration */
if( seconds != service_time )
{
/* need to call calibrate here */
Calibrate( service_time, cmd_args.getOriginalArguments() );
/* won't return from this one, however
* some compilers require a return here */
return( false );
}
return( true );
}
// ////////////////////////////////////////////////////////////////////////////
// CamShiftTracker::CamShiftTracker()
//
// Constructor.
//
// ////////////////////////////////////////////////////////////////////////////
CamShiftTracker::CamShiftTracker(IUnknown *outer, HRESULT *phr)
: CUnknown(NAME("CamShift Tracker"), outer)
{
// set up default tracking params
int dims[] = { 20 };
set_hist_dims( 1, dims );
set_hist_bin_range( 0, 1, 180 );
set_threshold( 0 );
set_min_ch_val( 1, 20 ); // S MIN
set_max_ch_val( 1, 255 ); // S MAX
set_min_ch_val( 2, 40 ); // V MIN
set_max_ch_val( 2, 240 ); // V MAX
Calibrate();
}
void Start_TP() {
Init_MCU();
InitializeTouchPanel();
Delay_ms(1000);
TFT_Fill_Screen(0);
Calibrate();
TFT_Fill_Screen(0);
InitializeObjects();
display_width = Screen1.Width;
display_height = Screen1.Height;
DrawScreen(&Screen1);
}
void CTravelTab::OnTrack()
{
Main->bMemoryAccess = AdjustPrivileges();
UpdateData();
if (!m_bTrack)
{
Main->calibrated = false;
return;
}
if (!Main->calibrated)
Calibrate();
if (Main->calibrated)
AfxBeginThread(TrackingThread,NULL);
}
/*---------------------------------------------------------------------------*
* Task: main
*---------------------------------------------------------------------------*
* Description:
* In the uEZ system, main() is a task. Do not exit this task
* unless you want to the board to reset. This function should
* setup the system and then run the main loop of the program.
* Outputs:
* int -- Output error code
*---------------------------------------------------------------------------*/
void MainTask(void)
{
#if COMPILE_OPTION_USB_SDCARD_DISK
T_USBMSDriveCallbacks usbMSDiskCallbacks = {0};
#endif
printf("\f" PROJECT_NAME " " VERSION_AS_TEXT "\n\n"); // clear serial screen and put up banner
// Load the settings from non-volatile memory
if (NVSettingsLoad() != UEZ_ERROR_NONE) {
printf("EEPROM Settings\n");
NVSettingsInit();
NVSettingsSave();
}
// Start up the heart beat of the LED
UEZTaskCreate(Heartbeat, "Heart", 64, (void *)0, UEZ_PRIORITY_NORMAL, 0);
#if UEZ_ENABLE_USB_DEVICE_STACK
if (G_usbIsDevice) {
// Setup the USB MassStorage device to connect to MS1 (the SD Card)
USBMSDriveInitialize(
&usbMSDiskCallbacks,
0,
"MS1");
}
#endif
AudioStart();
// Force calibration?
Calibrate(CalibrateTestIfTouchscreenHeld());
if(!GUIManager_Start_emWin_PF()){
GUIManager_Create_All_Active_Windows_PF();
//start the main window.
if(G_SystemWindows_PF[HOME_SCREEN]){
GUIManager_Show_Window_PF(HOME_SCREEN);
GUI_ExecCreatedDialog(G_SystemWindows_PF[HOME_SCREEN]);
} else {
UEZFailureMsg("Failed to create windows!");
}
} else {
UEZFailureMsg("emWin Failed to Start!");
}
// We should not exit main unless we want to reset the board
}
MainWindow::MainWindow() : QMainWindow()
{
// settings_ = settings;
ui.setupUi(this);
qml_Page_ = new QDeclarativeView;
qml_Page_->setSource(QUrl("qrc:///ui/Checkerboard.qml"));
qml_Page_->setResizeMode(QDeclarativeView::SizeRootObjectToView);
this->game_page_ = new ViewManager();
QGraphicsObject *background = qml_Page_->rootObject();
// connect(background, SIGNAL(sizeChanged(int, int)),
// this, SIGNAL(sizeChanged(int, int)));
// connect(background, SIGNAL(calibrate()),
// this, SIGNAL(calibrate()));
// connect(background, SIGNAL(sizeChanged(int,int)),
// this, SLOT(UiSizeChanged()));
// connect(background, SIGNAL(resize()),
// this, SLOT(Resize()));
// connect(background, SIGNAL(detect()),
// this, SIGNAL(detect()));
// connect(background, SIGNAL(screencap()),
// this, SLOT(TakeScreenshot()));
connect(ui.Detect, SIGNAL(clicked()),
this, SIGNAL(Detect()));
connect(ui.Calibration, SIGNAL(clicked()),
this, SIGNAL(Calibrate()));
connect(ui.Toggle, SIGNAL(clicked()),
this, SLOT(Toggle()));
connect(ui.DiceRegister, SIGNAL(clicked()),
this, SIGNAL(DiceRegisterSetup()));
connect(background, SIGNAL(sizeChanged(int,int)),
this, SIGNAL(sizeChanged(int,int)));
connect(background, SIGNAL(sizeChanged(int,int)),
this, SLOT(UiSizeChanged()));
this->readSettings();
emit ( sizeChanged( this->width()/150, this->height()/150));
// setCentralWidget(this->qml_page);
ui.stackedWidget->addWidget(qml_Page_);
ui.stackedWidget->addWidget(game_page_);
ui.stackedWidget->setCurrentWidget(qml_Page_);
}
void IMU::Loop( float dt )
{
if ( mState == Off ) {
// Nothing to do
} else if ( mState == Calibrating or mState == CalibratingAll ) {
Calibrate( dt, ( mState == CalibratingAll ) );
} else if ( mState == CalibrationDone ) {
mState = Running;
} else if ( mState == Running ) {
UpdateAttitude( dt );
if ( mPositionUpdate ) {
mPositionUpdateMutex.lock();
mPositionUpdate = false;
mPositionUpdateMutex.unlock();
UpdatePosition( dt );
}
}
}
void Adc_t::StartMeasurement() {
// Stop ADC
if(IsEnabled()) {
SET_BIT(ADC1->CR, ADC_CR_ADSTP); // Stop any ongoing conversion
while(READ_BIT(ADC1->CR, ADC_CR_ADSTP) != 0); // Let it to complete
Disable();
while(IsEnabled()); // Let it to complete
}
Calibrate();
__NOP(); __NOP(); __NOP(); __NOP(); // ADEN bit cannot be set during ADCAL=1 and 4 ADC clock cycle after the ADCAL bit is cleared by hardware(end of the calibration).
// Start ADC
SET_BIT(ADC1->ISR, ADC_ISR_ADRDY); // Clear ADRDY bit by writing 1 to it
SET_BIT(ADC1->CR, ADC_CR_ADEN); // Enable ADC
while(READ_BIT(ADC1->ISR, ADC_ISR_ADRDY) == 0); // Let it to complete
// Setup oversampler
#if ADC_OVERSAMPLING_RATIO == 1
ADC1->CFGR2 = 0; // Oversampler disabled
#elif ADC_OVERSAMPLING_RATIO == 2
ADC1->CFGR2 = (0b0001 << 5) | (0b000 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 4
ADC1->CFGR2 = (0b0010 << 5) | (0b001 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 8
ADC1->CFGR2 = (0b0011 << 5) | (0b010 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 16
ADC1->CFGR2 = (0b0100 << 5) | (0b011 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 32
ADC1->CFGR2 = (0b0101 << 5) | (0b100 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 64
ADC1->CFGR2 = (0b0110 << 5) | (0b101 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 128
ADC1->CFGR2 = (0b0111 << 5) | (0b110 << 2) | ADC_CFGR2_ROVSE;
#elif ADC_OVERSAMPLING_RATIO == 256
ADC1->CFGR2 = (0b1000 << 5) | (0b111 << 2) | ADC_CFGR2_ROVSE;
#endif
// Setup ADC. Do not set OVRMOD bit as it breaks sequence in case of DMA
SET_BIT(ADC1->CFGR, ADC_CFGR_DMAEN); // Enable DMA
// DMA
dmaStreamSetMemory0(ADC_DMA, IBuf);
dmaStreamSetTransactionSize(ADC_DMA, ADC_SEQ_LEN);
dmaStreamSetMode(ADC_DMA, ADC_DMA_MODE);
dmaStreamEnable(ADC_DMA);
// ADC
StartConversion();
}
void ADC::Enable(bool enable)
{
EnablePeripheralClock(enable);
if (enable)
{
*_pADC_CR2 |= ADC_CR2_ADON;
Calibrate();
_initialized = true;
}
else
{
*_pADC_CR2 &= ~ADC_CR2_ADON;
_initialized = false;
_emptySequence = true;
_emptyJSequence = true;
}
}
/**
* Constructor.
*/
ADIS16448_IMU::ADIS16448_IMU() : m_spi(SPI::Port::kMXP) {
m_spi.SetClockRate(1000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnFalling();
m_spi.SetClockActiveLow();
m_spi.SetChipSelectActiveLow();
ReadRegister(kRegPROD_ID); // dummy read
// Validate the part ID
if (ReadRegister(kRegPROD_ID) != 16448) {
DriverStation::ReportError("could not find ADIS16448");
return;
}
// Set IMU internal decimation to 102.4 SPS
WriteRegister(kRegSMPL_PRD, 769);
// Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
WriteRegister(kRegMSC_CTRL, 4);
// Configure IMU internal Bartlett filter
WriteRegister(kRegSENS_AVG, 1030);
m_cmd[0] = kGLOB_CMD;
m_cmd[1] = 0;
// Configure interrupt on MXP DIO0
m_interrupt.reset(new InterruptSource(10));
m_interrupt->RequestInterrupts();
m_interrupt->SetUpSourceEdge(false, true);
// Start monitoring thread
m_freed = false;
m_task = std::thread(&ADIS16448_IMU::Calculate, this);
Calibrate();
//HALReport(HALUsageReporting::kResourceType_ADIS16448, 0);
LiveWindow::GetInstance()->AddSensor("ADIS16448_IMU", 0, this);
}
/**
* Gyro constructor on the specified SPI port.
*
* @param port The SPI port the gyro is attached to.
*/
ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnRising();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
// Validate the part ID
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
DriverStation::ReportError("could not find ADXRS450 gyro");
return;
}
m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c000000u, 0x04000000u,
10u, 16u, true, true);
Calibrate();
HALReport(HALUsageReporting::kResourceType_ADXRS450, port);
LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this);
}
Revolver::Revolver()
{
m_Addr = 0;
m_currentFilter = -1;
char str[80];
// Inicializamos
m_RPF_parameters.nfw = 1;
m_RPF_parameters.nfilt = MAX_FILTROS_DTA;
// filter steps
m_RPF_parameters.step = 800 / m_RPF_parameters.nfilt;
m_RPF_parameters.com = COM5;
m_RPF_parameters.baud = Baud19200;
m_RPF_parameters.hold = 1;
m_RPF_parameters.torque = 15984;
m_RPF_parameters.offset = 0;
m_RPF_parameters.calspd = 20000;
m_RPF_parameters.slope = 255;
m_RPF_parameters.stspd = 65535;
m_RPF_parameters.enspd = 6144;
m_RPF_parameters.feed = 1;
m_RPF_parameters.dly = 125;
//Cargamos
LoadCFG(m_RPF_parameters);
int er = SioReset(m_RPF_parameters.com, 64, 64);
if (er != 0)
{
if (er == IE_BADID)
error_fatal("Init Revolver", "Imposible abrir COM: Puerto inexistente", 0);
if (er == IE_OPEN)
error_fatal("Init Revolver", "Imposible abrir COM: Puerto ya abierto", 0);
if (er == IE_MEMORY)
error_fatal("Init Revolver", "Imposible abrir COM: No hay memoria suficiente", 0);
else
error_fatal("Init Revolver", "Imposible abrir COM", 0);
return;
}
er = SioBaud(m_RPF_parameters.com, m_RPF_parameters.baud);
if (er == IE_BAUDRATE )
{
error_fatal("Init Revolver", "Unsupported boud rate", 0);
return;
}
for(int n = 0; n < m_RPF_parameters.nfw; n++)
{
m_Addr = n;
// Hold
sprintf(str, "9%d", m_RPF_parameters.hold);
if (Cmd(str) == false)
{
AfxMessageBox("*** Init Revolver: No hay respuesta por el puerto COM, asegurese que el dispositivo esta conectado");
exit(-1);
}
// Torque value
sprintf(str, "3%04X", m_RPF_parameters.torque);
Cmd(str);
// Offset value
sprintf(str, "4%02X", m_RPF_parameters.offset + 127);
Cmd(str);
// Num. of filters
sprintf(str, "5%02X", m_RPF_parameters.nfilt);
Cmd(str);
// Num. of filters
sprintf(str, "6%03X", m_RPF_parameters.step);
Cmd(str);
// Calibration speed
sprintf(str, "8%04X", m_RPF_parameters.calspd);
Cmd(str);
// Slope
sprintf(str, "A%02X", m_RPF_parameters.slope);
Cmd(str);
// Start speed
sprintf(str, "B%04X", m_RPF_parameters.stspd);
Cmd(str);
// End speed
sprintf(str, "C%04X", m_RPF_parameters.enspd);
Cmd(str);
// Enable feedback
sprintf(str, "L%d", m_RPF_parameters.feed);
Cmd(str);
// Delay after pos.
sprintf(str, "K%04X", m_RPF_parameters.dly);
Cmd(str);
// Calibrate
Calibrate();
}
}
// Optional change of configuration (if different from default)
Revolver::Revolver(rpf_parameters& parameters)
{
m_Addr = 0;
m_currentFilter = -1;
char str[80];
m_RPF_parameters.nfw = parameters.nfw ;
m_RPF_parameters.nfilt = parameters.nfilt ;
// filter steps
m_RPF_parameters.step = parameters.step ;
m_RPF_parameters.com = parameters.com ;
m_RPF_parameters.baud = parameters.baud ;
m_RPF_parameters.hold = parameters.hold ;
m_RPF_parameters.torque = parameters.torque ;
m_RPF_parameters.offset = parameters.offset ;
m_RPF_parameters.calspd = parameters.calspd ;
m_RPF_parameters.slope = parameters.slope ;
m_RPF_parameters.stspd = parameters.stspd ;
m_RPF_parameters.enspd = parameters.enspd ;
m_RPF_parameters.feed = parameters.feed ;
m_RPF_parameters.dly = parameters.dly ;
int er = SioReset(m_RPF_parameters.com, 64, 64);
if (er != 0)
{
if (er == IE_BADID)
error_fatal("Init Revolver", "Imposible abrir COM: Puerto inexistente", 0);
if (er == IE_OPEN)
error_fatal("Init Revolver", "Imposible abrir COM: Puerto ya abierto", 0);
if (er == IE_MEMORY)
error_fatal("Init Revolver", "Imposible abrir COM: No hay memoria suficiente", 0);
else
error_fatal("Init Revolver", "Imposible abrir COM", 0);
return;
}
er = SioBaud(m_RPF_parameters.com, m_RPF_parameters.baud);
if (er == IE_BAUDRATE )
{
error_fatal("Init Revolver", "Unsupported boud rate", 0);
return;
}
for(int n = 0; n < m_RPF_parameters.nfw; n++)
{
m_Addr = n;
// Hold
sprintf(str, "9%d", m_RPF_parameters.hold);
Cmd(str);
// Torque value
sprintf(str, "3%04X", m_RPF_parameters.torque);
Cmd(str);
// Offset value
sprintf(str, "4%02X", m_RPF_parameters.offset + 127);
Cmd(str);
// Num. of filters
sprintf(str, "5%02X", m_RPF_parameters.nfilt);
Cmd(str);
// Num. of filters
sprintf(str, "6%03X", m_RPF_parameters.step);
Cmd(str);
// Calibration speed
sprintf(str, "8%04X", m_RPF_parameters.calspd);
Cmd(str);
// Slope
sprintf(str, "A%02X", m_RPF_parameters.slope);
Cmd(str);
// Start speed
sprintf(str, "B%04X", m_RPF_parameters.stspd);
Cmd(str);
// End speed
sprintf(str, "C%04X", m_RPF_parameters.enspd);
Cmd(str);
// Enable feedback
sprintf(str, "L%d", m_RPF_parameters.feed);
Cmd(str);
// Delay after pos.
sprintf(str, "K%04X", m_RPF_parameters.dly);
Cmd(str);
// Calibrate
Calibrate();
}
}
void PID_MENU(void){
unsigned char mode=0;
unsigned long count=0;
unsigned char kval;
while(1){
if(SW1==0){
while(SW1==0); //wait for switch release
mode++;
if (mode>2) mode=0;
}
else if(SW2==0 ){
while(SW2==0); //wait for switch release
switch (mode){
case 0: return; //start
case 1: junction_mode^=1; break; //mode change line follow/ junction count
case 2: Calibrate(); break; //calibrate LSA08
}
}
//LCD display
lcd_goto(0);
switch (mode){
case 0: lcd_putstr("Start\n");
lcd_putstr("PressSW2");
break;
case 1: lcd_putstr("Mode \n");
if(junction_mode) lcd_putstr("Junction");
else lcd_putstr("Line Flw");
break;
case 2: lcd_putstr("Calb \n");
lcd_putstr("PressSW2");
break;
}
}
// out:
// if(SW1==0){
// count=0;
// while(SW1==0){
// count++;
//
// if(count>60000){
// mode++;
// if(mode>5) mode=0;
// lcd_clr();
//
// while(SW1==0);
// goto out;
// }
// }
//
// switch (mode){
// case 0: Kp-=0.1;
// if(Kp<0) Kp=0;
// break;
// case 1: Ki-=0.001;
// if(Ki<0) Ki=0;
// break;
// case 2: Kd-=1.0;
// if(Kd<0.0) Kd=0.0;
// break;
//
// }
//
// }
//
//
// if(SW2==0){
//
// while(SW2==0);
//
// switch (mode){
// case 0: Kp+=0.1;
// if(Kp>9) Kp=9;
// break;
// case 1: Ki+=0.001;
// if(Ki>5) Ki=5;
// break;
// case 2: Kd+=1.0;
// if(Kd>50.0) Kd=50.0;
// break;
// case 3: return;
// case 5: Calibrate(); break;
// case 4: junction_mode^=1; break;
//
// }
// }
//
//
// lcd_goto(0);
// switch(mode){
//
// case 0: lcd_putstr("Kp \n");
// kval=(unsigned char)Kp;
// lcd_num(kval,1); lcd_putchar('.');
// lcd_num((Kp-(float)kval)*10,1);
// lcd_putchar(' ');
// break;
// case 1: lcd_putstr("Ki \n");lcd_goto(20);
// kval=(unsigned char)Ki;
// lcd_num(kval,1); lcd_putchar('.');
// if((Ki-(float)kval)*1000<100) lcd_putchar('0');
// if((Ki-(float)kval)*1000<10) lcd_putchar('0');
// lcd_num((Ki-(float)kval)*1000,3);
// lcd_putchar(' ');
// break;
// case 2:lcd_putstr("Kd \n");lcd_goto(20);
// kval=(unsigned char)Kd;
// lcd_num(kval,3);
// lcd_putchar(' ');
// break;
// case 3: lcd_putstr("start\n");
// lcd_putstr(" ");
// break;
// case 5: lcd_putstr("calb\n");
// lcd_putstr(" ");
// break;
// case 4: lcd_putstr("mode\n");
// if(junction_mode) lcd_putstr("junction");
// else lcd_putstr("line_flw");
// break;
// }
//
// }
}
BOOL CGretagScan::StartComm(void)
{
char data[200];
int datalen = 200;
int code[5];
// check device type
if(!Command("; 43\r\n", 6, data, datalen, 5) )
{
ErrorMessage(ERR_NODEVICE);
return FALSE;
}
if(!sscanf_n_i(data, 27, 1, &code[0]) || code[0] != 2)
{
ErrorMessage(ERR_NODEVICE);
return FALSE;
}
//reset
if(!Command("; 90 1 4 5\r\n", 12, data, datalen, 5) )
{
ErrorMessage(ERR_DEVICE_READ);
return FALSE;
}
if(!sscanf_n_i(data,1,2,code))
{
ErrorMessage(ERR_DEVICE_RESET);
return FALSE;
}
if( !CheckError(code) )
return FALSE;
//set measurment type
if(!Command("; 77 155\r\n", 10, data, datalen, 5) )
{
ErrorMessage(ERR_DEVICE_READ);
return FALSE;
}
if(!sscanf_n_i(data,1,2,code))
{
ErrorMessage(ERR_DEVICE_MEASUREMENT_TYPE);
return FALSE;
}
if( !CheckError(code) )
return FALSE;
// set the illuminante, angle
if(!Command("; 22 1 1 3 0\r\n", 14, data, datalen, 5) )
{
ErrorMessage(ERR_DEVICE_READ);
return FALSE;
}
if(!sscanf_n_i(data,1,2,code))
{
ErrorMessage(ERR_DEVICE_ILLUMINATION);
return FALSE;
}
if( !CheckError(code) )
return FALSE;
//calibrate before reading
if( !Calibrate() )
return FALSE;
//align table with patch target
if( !AlignTable() )
return FALSE;
return TRUE;
}
