int main(void) {
SERIAL_Init();
TIMERS_Init();
char i, j = 0;
int k, l = 0;
int time = GetTime();
INTEnableSystemMultiVectoredInt();
AD_Init(IR_PINS);
IR_Init();
while (1) {
k = ReadADPin(IR_LEFT);
l = ReadADPin(IR_RIGHT);
char leftTrig = '_';
char rightTrig = '_';
if (IR_LeftTriggered())
leftTrig = 'x';
if (IR_RightTriggered())
rightTrig = 'x';
//if (time > GetTime() + 500) {
if (IsTransmitEmpty()) {
printf("\n %cLeft : %d \n %cRight : %d",leftTrig, k, rightTrig, l);
}
wait();
//time = GetTime();
//}
//while (!IsTransmitEmpty()); // bad, this is blocking code
}
return 0;
}
char main(){
int i;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
Drive_Init();
dbprintf("\nHello World");
AD_Init(BAT_VOLTAGE);
LED_Init(LED_BANK3);
LED_OffBank(LED_BANK3, 0xf);
wait();
int pwm;
while(1) {
//Drive_Stop();
wait();
//Drive_Forward(MIN_SPEED);
printf("\nFORWARD! speed=%u pwm=%u", motorSpeed[A], motorPWMValue[A]);
//for (i = 0; i < 100; i++)
// wait();
Drive_Stop();
Drive_Pivot(left, 1);
for (i = 0; i < 1; i++)
wait();
Drive_Stop();
Drive_Pivot(right, 1);
for (i = 0; i < 1; i++)
wait();
Drive_Stop();
for (i = 0; i < 1; i++)
wait();
}
}
int main(void) {
unsigned int wait = 0;
int readcount = 0;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
mJTAGPortEnable(0);
printf("\r\nUno A/D Test Harness\r\nThis will initialize all A/D pins and read them %d times", TIMES_TO_READ);
printf("Value of pcfg before test: %X",AD1PCFG);
AD_Init(AD_PORTV3 | AD_PORTV4 | AD_PORTV5 | AD_PORTV6 | AD_PORTW4 | AD_PORTW3 | BAT_VOLTAGE | PINA);
unsigned char cur = 0;
while (readcount <= TIMES_TO_READ) {
for (wait = 0; wait <= 100000; wait++)
asm("nop");
printf("\r\nAN0\tAN1\tAN2\tAN3\tAN4\tAN5\tAN11\tAN10\n");
printf("%d\t", ReadADPin(PINA));
printf("%d\t", ReadADPin(BAT_VOLTAGE));
printf("%d\t", ReadADPin(AD_PORTV3));
printf("%d\t", ReadADPin(AD_PORTV4));
printf("%d\t", ReadADPin(AD_PORTV5));
printf("%d\t", ReadADPin(AD_PORTV6));
printf("%d\t", ReadADPin(AD_PORTW4));
printf("%d\t", ReadADPin(AD_PORTW3));
readcount++;
}
printf("Done Reading Them\r\n");
AD_End();
printf("Value of pcfg after test: %X",AD1PCFG);
return 0;
}
int main(void) {
unsigned int wait = 0;
int readcount = 0;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
mJTAGPortEnable(0);
printf("\r\nUno A/D Test Harness\r\nThis will initialize all A/D pins and read them %d times", TIMES_TO_READ);
printf("Value of pcfg before test: %X",AD1PCFG);
AD_Init(AD_PORTV3 | AD_PORTV4 | AD_PORTV5 | AD_PORTV6 | AD_PORTV7 | AD_PORTV8 | AD_PORTW3 | AD_PORTW4 | AD_PORTW5 | AD_PORTW6 | AD_PORTW7 | AD_PORTW8 | BAT_VOLTAGE);
char numtoread = 13;
unsigned char cur = 0;
while (readcount <= TIMES_TO_READ) {
for (wait = 0; wait <= 100000; wait++)
asm("nop");
// printf("\r\nAN2\tAN3\tAN4\tAN5\tAN8\tAN9\tAN11\tAN10\tAN13\tAN12\tAN15\tAN14\tAN1\n");
for (cur = 0; cur < numtoread; cur++) {
printf("%d\t", ReadADPin(1 << cur));
}
printf("\r\n");
readcount++;
}
printf("Done Reading Them\r\n");
AD_End();
printf("Value of pcfg after test: %X",AD1PCFG);
return 0;
}
/****************************************************************************
Function
Roach_Init
Parameters
None.
Returns
SUCCESS if operation successful
ERROR otherwise
Description
Performs all the initialization necessary for the roach.
this includes initializing the PWM module, the A/D converter, the
data directions on some pins, and setting the initial motor directions.
Notes
None.
Author
Max Dunne, 2012.01.06
****************************************************************************/
char Roach_Init(void) {
//Initialize the serial port
SERIAL_Init();
TIMERS_Init();
//set the control pins for the motors
PWM_Init(LEFT_PWM | RIGHT_PWM, 200);
LEFT_DIR_TRIS = 0;
LEFT_DIR_INV_TRIS = 0;
RIGHT_DIR_TRIS = 0;
RIGHT_DIR_INV_TRIS = 0;
LEFT_DIR = 0;
LEFT_DIR_INV = ~LEFT_DIR;
RIGHT_DIR = 0;
RIGHT_DIR_INV = ~RIGHT_DIR;
//set up the hall effect and divorce all the A/D pins
AD1PCFG = 0xFF;
HALL_FRONT_LEFT_TRIS = 1;
HALL_FRONT_RIGHT_TRIS = 1;
HALL_REAR_RIGHT_TRIS = 1;
HALL_REAR_LEFT_TRIS = 1;
//Initialize the light sensor
AD_Init(LIGHT_SENSOR);
//enable interrupts
INTEnableSystemMultiVectoredInt();
}
void BOARD_Init() {
unsigned int dma_status;
unsigned int int_status;
//DEVCFG1bits.FCKSM = 0;
//mSYSTEMUnlock(int_status, dma_status);
//DEVCFG1bits.FCKSM = 0;
//DEVCFG2bits.FPLLIDIV=1;
SYSKEY = 0;
SYSKEY = 0xAA996655;
SYSKEY = 0x556699AA;
CFGCONbits.IOLOCK = 0;
SYSKEY = 0;
SYSTEMConfig(SYSTEM_CLOCK, SYS_CFG_ALL);
ANSELA=0;
ANSELB=0;
ANSELC=0;
//OSCConfig(OSC_FRC_PLL, OSC_PLL_MULT_20, OSC_PLL_POST_1, OSC_FRC_POST_1);
//OSCCONbits.NOSC=0x1;
OSCSetPBDIV(OSC_PB_DIV_1);
//mSYSTEMLock(int_status, dma_status);
// AD1PCFG = 0xffff;
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
SERIAL_Init();
//printf("This code compiled at %s on %s\r\n", __TIME__, __DATE__);
}
int main(void) {
SERIAL_Init();
int i;
char keyPressed;
INTEnableSystemMultiVectoredInt();
if (Gate_Init() == SUCCESS) {
printf("Succesfully initialized servo (%x)", SERVO);
}
else {
printf("Failed to initialize servo (%x)", SERVO);
}
Gate_Close();
printf("\nHello World!");
DELAY();
while(1) {
keyPressed = GetChar();
if (keyPressed == 'c'){
Gate_Close();
}
else if (keyPressed == 'o'){
Gate_Open();
}
} // end of loop
Gate_End();
return 0;
}
int main(void) {
int i;
unsigned short pattern;
char j,k;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
printf("\nHello World!");
Stepper_Init(100);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,420);
for (j=0;j<10;j++) {
DELAY();
printf(".");
DELAY();
printf("\t%u",Stepper_GetRemainingCount());
}
while(Stepper_GetRemainingCount() > 1) {
printf("o");
DELAY();
}
printf("\nForward steps done");
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("+");
}
Stepper_End();
TRIS_COIL_A_DIRECTION = 0;
TRIS_COIL_A_ENABLE = 0;
TRIS_COIL_B_DIRECTION = 0;
TRIS_COIL_B_ENABLE = 0;
ShutDownDrive();
COIL_A_ENABLE = 0;
COIL_A_DIRECTION = 0;
COIL_B_ENABLE = 0;
COIL_B_DIRECTION = 0;
printf("\n Turning on COIL_B_DIRECTION (PortZ-07");
for (j=0;j<10;j++) {
printf("o");
DELAY();
}
while (1) {
COIL_B_DIRECTION ^= 1;
DELAY();
printf(".");
}
return 0;
}
int main(void) {
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
printf("\r\nUno Serial Test Harness\r\nAfter this Message the terminal should mirror anything you type.\r\n");
unsigned char ch = 0;
while (1) {
if (IsTransmitEmpty() == TRUE)
if (IsReceiveEmpty() == FALSE)
PutChar(GetChar());
}
return 0;
}
int main(void) {
int i;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
//enable LED bank 2
TRISFbits.TRISF6 = 0;
TRISGbits.TRISG7 = 0;
TRISDbits.TRISD7 = 0;
TRISGbits.TRISG8 = 0;
printf("\nHello World!");
Stepper_Init(STEPPER_SPEED);
Stepper_SetMode(STEPPER_MODE);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,220);
while(Stepper_GetRemainingCount() > 1) {
printf("\t%u,speed=%u",Stepper_GetRemainingCount(),Stepper_GetSpeed());
DELAY();
#ifdef STEPPER_RAMPS
if (Stepper_GetRemainingCount() % 5) {
Stepper_ChangeStepRate( (Stepper_GetSpeed() / 4) + Stepper_GetSpeed() );
}
#endif
}
printf("\nForward steps done");
DELAY();
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("\t%u",Stepper_GetRemainingCount());
DELAY();
}
Stepper_End();
return 0;
}
int main(void) {
int i = 0;
SERIAL_Init();
TIMERS_Init();
INTEnableSystemMultiVectoredInt();
printf("\r\nUno Timers Test Harness\r\n");
printf("Setting each timer for one second longer than the last and waiting for all to expire. There are %d available timers\r\n", TIMERS_IN_TEST);
for (i = 0; i <= TIMERS_IN_TEST; i++) {
InitTimer(i, (i + 1)*1000); //for second scale
}
while (IsTimerActive(TIMERS_IN_TEST - 1) == TIMER_ACTIVE) {
for (i = 0; i <= TIMERS_IN_TEST; i++) {
if (IsTimerExpired(i) == TIMER_EXPIRED) {
printf("Timer %d has expired and the free running counter is at %d\r\n", i, GetTime());
ClearTimerExpired(i);
}
}
}
printf("All timers have ended\r\n");
printf("Setting and starting 1st timer to 2 seconds using alternative method. \r\n");
SetTimer(0, 2000);
StartTimer(0);
while (IsTimerExpired(0) != TIMER_EXPIRED);
printf("2 seconds should have elapsed\r\n");
printf("Starting 1st timer for 8 seconds but also starting 2nd timer for 4 second\r\n");
InitTimer(0, 8000);
InitTimer(1, 4000);
while (IsTimerExpired(1) != TIMER_EXPIRED);
printf("4 seconds have passed and now stopping 1st timer\r\n");
StopTimer(0);
printf("Waiting 6 seconds to verifiy that 1st timer has indeed stopped\r\n");
InitTimer(1, 3000);
i = 0;
while (IsTimerActive(1) == TIMER_ACTIVE) {
if (IsTimerExpired(0) == TIMER_EXPIRED) {
i++;
ClearTimerExpired(0);
}
}
if (i == 0) {
printf("Timer did not expire, module working correctly\r\n");
} else {
printf("Timer did expire, module not working correctly\r\n");
}
return 0;
}
/*============================================================================
Name : QDebug_Init
------------------------------------------------------------------------------
Purpose : Initialize Debug Interface
Input : n/a
Output : n/a
Notes : Must be called before using any other QDebug API.
============================================================================*/
void QDebug_Init(void)
{
int16_t touch_ret;
#if (defined QDEBUG_SPI)
SPI_Master_Init();
#elif (defined QDEBUG_SERIAL)
SERIAL_Init();
#endif
touch_ret = QDEBUG_GET_LIBINFO_FUNC(&QDEBUG_LIBINFO);
if(touch_ret != TOUCH_SUCCESS)
{
while(1);
}
Init_Buffers();
}
int main(void) {
BOARD_Init();
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SERIAL_Init();
SPI_init(SPI_SS1_CLKRATE, 0, 0);
int i, j;
printf("SPI module test begins...\n");
//* SPI_changeClkRate() test: it works period of clock is 5MHz vs 500kHz
uint8_t testDataAry[] = {0x0B, 0x0E, 0x0E, 0x0F};
while (1) {
SPI_write(SPI_SS1_ID, testDataAry, 4);
for (i = 0; i < 3000000; i++);
SPI_write(SPI_SS2_ID, testDataAry, 4);
for (i = 0; i < 3000000; i++);
}
//*/
printf("test ended\n");
return (EXIT_SUCCESS);
}
int main(void) {
BOARD_Init();
SERIAL_Init();
//Init the motors, duh
initMotors();
char input;
dbprintf("Welcome to the Motor Test Harness\n");
dbprintf("Speed: 0-10, f: forward, s: backward\n");
dbprintf("r: right motor, l: left motor, b: battery\n");
while (1) {
input = GetChar();
char_input(input);
DELAY(A_BIT)
}
return 0;
}
void vDos_Init(void)
{
hideWinTill = GetTickCount()+2500; // Auto hidden till first keyboard check, parachute at 2.5 secs
LOG_MSG("vDos version: %s", vDosVersion);
#ifndef WITHIRQ1
// Wil have been called earlier in starup if WITHIRQ1 is defined
vDos_LoadConfig();
#endif
GUI_StartUp();
IO_Init();
PAGING_Init();
MEM_Init();
CALLBACK_Init();
PIC_Init();
PROGRAMS_Init();
TIMER_Init();
// CMOS_Init();
VGA_Init();
CPU_Init();
KEYBOARD_Init();
BIOS_Init();
INT10_Init();
MOUSE_Init();
SERIAL_Init();
PARALLEL_Init();
printTimeout = ConfGetBool("timeout");
DOS_Init();
XMS_Init();
EMS_Init();
if (errorMess[0])
MessageBox(NULL, errorMess+1, "vDos: CONFIG.TXT has unresolved items", MB_OK|MB_ICONWARNING);
SHELL_Init(); // Start up main machine
}
int main(void) {
SERIAL_Init();
int Real_Rate=0;
char data_out=0;
Real_Rate=I2C_Init(400000);
//TRISGbits.TRISG9=0;
// _RG9=0;
//PORTClearBits(IOPORT_G,BIT_9);
INTEnableSystemMultiVectoredInt();
printf("\r\nAnima Sensor Test Board I2C driver test\r\nThis will verify I2C operation through use of a logic analyzer\r\n");
printf("Rate returned from I2C Module: %d\r\n",Real_Rate);
data_out=I2C_ReadReg(0x40,0x02);
printf("\r\n%d\r\n",data_out);
while(1);
unsigned char ch = 0;
while (1) {
if (IsTransmitEmpty() == TRUE)
if (IsReceiveEmpty() == FALSE)
PutChar(GetChar());
}
return 0;
}
int main(void)
{
//DDRB = 0x00; //DEFINE PORTB INPUT
//DDRB |= (1<<0);
//int i=0;
//char data[50];
//SERIAL_Init(9600,'D',1,8,'R');
//SPI_InitMaster(0,'M',8,'B',2);
//NRF_PrintRegister(SETUP_RETR);
//NRF_WriteRegister(SETUP_RETR, 0b00000000);
//NRF_PrintRegister(SETUP_RETR);
//NRF_ReadRegister(SETUP_RETR);
/*
SERIAL_SendCaracter('>');
while (1)
{
//PORTB |= (1<<0);
//_delay_ms(1000);
//PORTB &= ~(1<<0);
//_delay_ms(1000);
//_delay_ms(1000);
data[i] = SERIAL_READ();
SERIAL_SendCaracter(data[i]);
if(data[i] == 13){
data[i+1]=0;
SERIAL_SendCaracter(10);
SERIAL_SendCaracter(13);
SERIAL_SendStringLn(data);
SERIAL_SendCaracter('>');
i=-1;
}
i++;
}
*/
DDRB |=(1<<CE); //SET CE TO OUTPUT.
DDRB &= ~(1<< DDD2); //SET PD2 TO INPUT
PORTD |= (1<<PORTD2);//SET PD2 TO HIGH
EICRA |= (1<<ISC10);
EIMSK |= (1<<INT0);
sei();
SERIAL_Init(9600,'D',1,8,'R');
SPI_InitMaster(0,'M',8,'B',2);
char data[1];
NRF_SET_PTX();
SERIAL_SendCaracter('>');
while(1)
{
NRF_Control(STANBY);
NRF_FlushFifo(TX);
data[0] = SERIAL_READ();
if(data[0] == 13)
{
SERIAL_SendStringLn(data[0]);
}
SERIAL_SendCaracter(data[0]);
NRF_LoadTXPayload(data,1);
NRF_ActivateTransmission();
_delay_ms(1);
}
}
int main(void) {
// ----------------- Initialization --------------
SERIAL_Init();
AD_Init(POT_INPUT);
// Initialize interrupts
INTEnableSystemMultiVectoredInt();
RC_Init(RC_PORT);
unsigned int wait = 0;
for (wait = 0; wait <= 1000000; wait++)
asm("nop");
RC_SetPulseTime(RC_PORT, 2000);
printf("\nHello,...");
while (1) {
// Read and print potentiometer
unsigned int potValue = ReadPotentiometer();
//printf("\nPot. reading: %x", potValue);
// Pause if desired
#ifdef ADC_PAUSE
#endif
unsigned int newSpeed = potValue +1000;
printf("\nPot value to %u", potValue);
// bound it
newSpeed = max(newSpeed,MINPULSE);
newSpeed = min(newSpeed,MAXPULSE);
// effect the motor
if (RC_SetPulseTime(RC_PORT, newSpeed) == SUCCESS) {
printf("\nSuccessfully set PWM to %u", newSpeed);
}
else {
printf("\nFailed to set PWM to %u", newSpeed);
}
/*
char keyPressed = GetChar();
if (keyPressed != 0) {
/*
if (keyPressed == 'f') {
// forward
printf("\nforward");
if (DIRECTION != FORWARD)
DIRECTION = FORWARD;
}
else if(keyPressed == 'r') {
// reverse
printf("\nreverse");
if (DIRECTION != REVERSE)
DIRECTION = REVERSE;
}
else if(keyPressed == 'q') {
printf("\nGoodbye!");
PWM_End();
return 0;
}
if(keyPressed == 't') {
unsigned short int stringMax = 4;
unsigned short int i = 0;
char charNumber[1];
charNumber[0] = 0;
char stringNumber[stringMax+1];
printf("\nPlease enter a frequency:");
while(charNumber[0] != 46) {
charNumber[0] = GetChar();
//printf("\nGot %s", charNumber);
if ((charNumber[0] <= 57 && charNumber[0] >= 48)) {
stringNumber[i] = charNumber[0];
stringNumber[i+1] = '\0';
//printf("\nAppended %c to '%s'",charNumber[0],stringNumber);
//stringNumber = strcat(stringNumber, charNumber);
i++;
}
if (i >= stringMax)
break;
}
//printf("\ni=%u stmax=%u", i, stringMax);
printf("\nDone");
unsigned int newFreq = atoi(stringNumber);
newFreq = max(newFreq,MINPULSE);
newFreq = min(newFreq,MAXPULSE);
printf("\nPulse width set to %u",newFreq);
RC_SetPulseTime(RC_PORT,newFreq);
}
} // end of keyPressed
*/
while (!IsTransmitEmpty()); // bad, this is blocking code
} // end of while loop
return 0;
}
int main(void) {
int i;
unsigned short pattern;
char j,k;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
//enable LED bank 2
TRISFbits.TRISF6 = 0;
TRISGbits.TRISG7 = 0;
TRISDbits.TRISD7 = 0;
TRISGbits.TRISG8 = 0;
printf("\nHello World!");
Stepper_Init(stepSpeed);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,420);
while(Stepper_GetRemainingCount() > 1) {
printf("\t%u,%u",Stepper_GetRemainingCount(),stepSpeed);
DELAY();
#ifdef STEPPER_RAMPS
if (Stepper_GetRemainingCount() % 10) {
Stepper_SetSpeed( (stepSpeed / 4) + stepSpeed );
}
#endif
}
printf("\nForward steps done");
DELAY();
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("\t%u",Stepper_GetRemainingCount());
DELAY();
}
Stepper_End();
TRIS_COIL_A_DIRECTION = 0;
TRIS_COIL_A_ENABLE = 0;
TRIS_COIL_B_DIRECTION = 0;
TRIS_COIL_B_ENABLE = 0;
ShutDownDrive();
ADisable();
ADirectionOff();
BDisable();
BDirectionOff();
printf("\n Turning on COIL_B_DIRECTION (PortZ-07");
for (j=0;j<10;j++) {
printf("o");
DELAY();
}
while (1) {
COIL_B_DIRECTION ^= 1;
DELAY();
printf(".");
}
return 0;
}
int main(void)
{
BOARD_Init();
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SERIAL_Init();
SPI_Init(SPI_SS1_CLKRATE, 0, 0);
TIMERS_Init();
printf("program begins...\n");
uint16_t i, j;
/* timer test: sucess
SET_OUTPUT_PIN(D, 10);
TIMERS_Init();
while (1) {
WRITE_HIGH(D, 10);
InitTimer(1, 100);
while (IsTimerActive(1));
WRITE_LOW(D, 10);
InitTimer(1, 100);
while (IsTimerActive(1)) ;
}
//*/
/* SPI read test 1: need to get 0x70 to ensure SPI reading is good
printf("IMU: SPI read test 1\n");
IMU_Init();
uint8_t id;
while (1) {
id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
printf( "id should be 0x70: 0x%x\n", id );
InitTimer(1, 1000);
while (IsTimerActive(1)) ;
}
//*/
/* SPI read test 2: need to get 0x70 to ensure SPI reading is good
printf("IMU: SPI read test 2\n");
IMU_Init();
uint8_t id;
while (1) {
MPU6500_readBytes(SPI_SS2_ID, MPU6500_RA_WHO_AM_I, 1, &id);
printf( "id should be 0x70: 0x%x\n", id );
InitTimer(1, 1000);
while (IsTimerActive(1)) ;
}
//*/
/* inv_mpu test: porting invense IMU library
printf("inv_mpu test: read test\n");
uint8_t buf[3] = {0};
uint8_t flag;
while (1) {
flag = mpu_read_reg(MPU6500_RA_WHO_AM_I, buf);
printf("flag: %d\n", flag);
printf( "id should be 0x70: 0x%x\n", buf[0]);
InitTimer(2, 1000);
while (IsTimerActive(2)) {
//printf("timer is still active\n");
}
}
//*/
/* porting inv_mpu test: init and read gyro
printf("inv_mpu test: read test\n");
uint16_t buf[3] = {0};
uint8_t flag = mpu_init(0);
mpu_reset_fifo();
mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
printf("flag: %d\n", flag);
uint16_t gyro[3], accel[3];
// flag = mpu_run_6500_self_test(gyro, accel, 0);
printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
while (1) {
flag = mpu_get_gyro_reg(buf, 0);
printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, buf[0], buf[1], buf[2]);
mpu_get_accel_reg(buf, 0);
printf(", accel: % 5d, % 5d, % 5d\n", buf[0], buf[1], buf[2]);
InitTimer(2, 1000);
while (IsTimerActive(2));
}
//*/
/* test: reading multiple times from the same register: the internal pointer +1 when it read once
int vals;
uint16_t gyro[3], accel[3];
vals = mympu_open(200);
printf("MPU Init: %d\n", vals);
struct s_mympu mympu;
uint8_t flag;
while(1) {
mpu_get_accel_reg(accel, 0);
printf("all accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
uint8_t tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_H);
accel[0] = tmp << 8;
tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_L);
accel[0] |= tmp;
tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_H);
accel[1] = tmp << 8;
tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_L);
accel[1] |= tmp;
tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_H);
accel[2] = tmp << 8;
tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_L);
accel[2] |= tmp;
printf("byte accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
InitTimer(2, 100);
while (IsTimerActive(2));
}
//*/
/* test: reading multiple times from the same register: the internal pointer +1 when it read once
int vals;
uint8_t buf[20] = {0};
uint16_t gyro[3] = {0}, accel[3] = {0};
vals = mympu_open(200);
printf("MPU Init: %d\n", vals);
//IMU_Init();
uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
printf( "id should be 0x70: 0x%x\n", id);
struct s_mympu mympu = {0};
int flag;
//MPU6500_writeOneByte( MPU6500_RA_USER_CTRL, 0x40 | 0x10 );
while(1) {
flag = mympu_update(&mympu);
//printf("yaw=[%d, %d], pitch=[%d,%d], roll=[%d,%d]\n", mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]);
//printf("flag=%d, yaw=% 11f, pitch=% 11f, roll=% 11f\n", flag, mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n",
mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]);
//flag = dmp_read_fifo(gyro, accel, buf, NULL,&vals,&vals);
//buf[0] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTH);
//buf[1] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTL);
//uint16_t count = (buf[0] << 8) | buf[1];
//printf("fifo count: %d\n", count);
//flag = mpu_read_fifo(gyro, accel, &vals, &vals, &vals);
//flag = mpu_read_fifo_stream(10, buf, &vals);
//read_fifo(buf, gyro, accel);
//printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
// printf(", accel: % 5d, % 5d, % 5", accel[0], accel[1], accel[2]);
//printf(", quat: %d, %d, %d, %d\n", buf[0], buf[1], buf[2], buf[3]);
InitTimer(2, 50);
while (IsTimerActive(2));
}
//*/
/* test: calibration by adding offset
int vals = mympu_open(200);
printf("MPU Init: %d\n", vals);
uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
printf( "id should be 0x70: 0x%x\n", id);
struct s_mympu mympu = {0};
int flag;
printf("waiting for initial drift\n");
InitTimer(2, 20000); // wait for 20s to pass initial drift
while (IsTimerActive(2));
printf("get average offset\n");
float offsetX = 0, offsetY = 0, offsetZ = 0;
float offsetPrev[3] = {0};
float alpha = 0;
uint8_t k;
for (k=1; k <= 200; k++) {
flag = mympu_update(&mympu);
//printf("yaw=% 3.3f, pitch=% 3.3f, roll=% 3.3f\n",
//mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
alpha = ((float) k - 1.) / k;
offsetX = alpha*offsetPrev[0] + (1. - alpha) * mympu.ypr[0];
offsetY = alpha*offsetPrev[1] + (1. - alpha) * mympu.ypr[1];
offsetZ = alpha*offsetPrev[2] + (1. - alpha) * mympu.ypr[2];
offsetPrev[0] = offsetX;
offsetPrev[1] = offsetY;
offsetPrev[2] = offsetZ;
InitTimer(2, 50);
while (IsTimerActive(2));
}
printf("average: x offset= %3.3f, y offset= %3.3f, z offset= %3.3f\n",
offsetX, offsetY, offsetZ);
//offsetX = abs(offsetX);
offsetY = abs(offsetY);
//offsetZ = abs(offsetZ);
while(1) {
flag = mympu_update(&mympu);
//printf("yaw=% 3.0f, pitch=% 3.0f, roll=% 3.0f\n",
//printf("%4.1f, %4.1f, %4.1f\n", mympu.ypr[0] - offsetX, mympu.ypr[1] - offsetY, mympu.ypr[2] - offsetZ);
printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n", mympu.ypr[0] - offsetX, mympu.gyro[0], mympu.ypr[1] - offsetY, mympu.gyro[1], mympu.ypr[2] - offsetZ, mympu.gyro[2]);
InitTimer(2, 50);
while (IsTimerActive(2));
}
//*/
//* test: two SPI devices, OLED and IMU
int vals = mympu_open(10);
OLED_Init();
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 0x2710);
ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
printf("MPU Init: %d\n", vals);
uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
printf( "id should be 0x70: 0x%x\n", id);
UG_SetForecolor(0x0F);
UG_SetBackcolor(0x00);
UG_FontSelect(FONT_6X8);
char str[50] = "";
while(1) {
//mympu_update(&mympu);
sprintf(str, "%.3f, %.3f, %.3f\n", mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
printf("%s\n",str);
flag_writingScreen = 1;
UG_PutString(0, 0, str);
flag_writingScreen = 0;
InitTimer(1, 200);
while (IsTimerActive(1));
}
//*/
/*
flag = mpu_run_6500_self_test(gyro, accel, 0);
printf("\nflag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_H, (accel[0] & 0x7F80) >> 8);
MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_L_TC, accel[0] & 0x7F);
MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_H, (accel[1] & 0x7F80) >> 8);
MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_L_TC, accel[1] & 0x7F);
MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_H, (accel[2] & 0x7F80) >> 8);
MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_L_TC, accel[2] & 0x7F);
MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRH, gyro[0] >> 8);
MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRL, gyro[0] & 0xFF);
MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRH, gyro[1] >> 8);
MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRL, gyro[1] & 0xFF);
MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRH, gyro[2] >> 8);
MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRL, gyro[2] & 0xFF);
//*/
printf("program ended...\n");
return 0;
}
int main(void) {
//unsigned char size = 0, i;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
TIMERS_Init();
printf("Uno32 FreeScale Magnetometer test\r\n");
printf("Size of short:%d\r\n", sizeof (short));
printf("Size of char:%d\r\n", sizeof (char));
printf("Size of int:%d\r\n", sizeof (int));
//while(1);
free_mag_init();
//printf("Scale is set to %d \r\n",free_mag_GetScale());
//free_mag_SetScale(free_mag_4GSCALE);
//printf("Scale is set to %d and should be %d\r\n",free_mag_GetScale(),free_mag_4GSCALE);
//free_mag_SetScale(free_mag_2GSCALE);
//printf("Scale is set to %d and should be %d\r\n",free_mag_GetScale(),free_mag_2GSCALE);
printf("Rate is set to %d \r\n", free_mag_GetRate());
free_mag_SetRate(FREE_MAG_0P08HERTZ_128_OVERRATIO);
printf("Rate is set to %d and should be %d\r\n", free_mag_GetRate(), FREE_MAG_0P08HERTZ_128_OVERRATIO);
//InitTimer(0, 1000);
//while (!IsTimerExpired(0));
free_mag_SetRate(FREE_MAG_40HERTZ_32_OVERRATIO);
printf("Rate is set to %d and should be %d\r\n", free_mag_GetRate(), FREE_MAG_40HERTZ_32_OVERRATIO);
while (1);
char humanread = 1;
int i, j, data;
//for (i = 0; i != 1600; i++)
//for (j = 0; j != 100; j++)
//Nop();
DELAY();
short AxisData[3];
while (1) {
//if (UART2HalfEmpty()){
//printf("%d",IsTransmitEmpty());
//DELAY();
if (humanread == 1) {
if (IsTransmitEmpty()) {
// printf("Cur X: %d \tCur Y: %d \tCur Z: %d\r\n", free_mag_GetXData(), free_mag_GetYData(), free_mag_GetZData());
free_mag_GetTriplet(AxisData);
printf("Cur X: %d \tCur Y: %d \tCur Z: %d\tTotal: %d\r\n", AxisData[0], AxisData[1], AxisData[2], AxisData[0] * AxisData[0] + AxisData[1] * AxisData[1] + AxisData[2] * AxisData[2]);
}
} else {
//data=free_mag_GetXData();
//UART2PutChar('$');
//UART2PutChar('#');
//UART2PutChar(data >> 8);
//UART2PutChar(data & 0x00FF);
//UART2PutChar('\r');
//UART2PutChar('\n');
}
//for (i = 0; i != -1; i++)
//Nop();
//}
/*while(!UART2IsEmpty())
UART2PutChar(UART2GetChar());*/
}
}
int main(void) {
//unsigned char size = 0, i;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
//UART2PutChar('a');
printf("sUno32 analog Accelerometer test\r\nAfter confirming settings reading will be taken\r\n");
analog_accel_init();
//while(1);
//printf("Scale is set to %d \r\n",analog_GetScale());
//analog_SetScale(analog_3GSCALE);
//while(1);
//printf("Scale is set to %d and should be %d\r\n", analog_GetScale(), analog_3GSCALE);
//analog_SetScale(analog_2GSCALE);
//printf("Scale is set to %d and should be %d\r\n", analog_GetScale(), analog_2GSCALE);
printf("Rate is set to %d \r\n", analog_GetRate());
//analog_SetRate(analog_300HERTZ);
printf("Rate is set to %d and should be %d\r\n", analog_GetRate(), analog_300HERTZ);
/*
analog_SetRate(analog_800HERTZ);
printf("Rate is set to %d and should be %d\r\n",analog_GetRate(),analog_800HERTZ);
*/
char humanread = 1;
int maxX = analog_GetXData(), maxY = analog_GetYData(), maxZ = analog_GetZData();
int minX = maxX, minY = maxY, minZ = maxZ;
int curval=0;
int i, j, data;
/*for(i=0; i!=-1; i++)
for(j=0; j!=100; j++)
Nop();
printf("Scale is set to %d and should be %d\r\n",analog_GetScale(),analog_3GSCALE);*/
for (i = 0; i != 1000; i++)
for (j = 0; j != 100; j++)
Nop();
while (1) {
if (IsTransmitEmpty()){
if (humanread == 1) {
printf("Cur X: %d \tCur Y: %d \tCur Z: %d\r\n", analog_GetXData(), analog_GetYData(), analog_GetZData());
//printf("combined z: %D\t\t",analog_GetZData());
} else {
printf("max X: %d \tmax Y: %d \tmax Z: %d \tmin X: %d \tmin Y: %d min Z: %d\r\n", maxX, maxY, maxZ, minX, minY, minZ);
curval = analog_GetXData();
if (curval < minX) {
minX = curval;
}
if (curval > maxX) {
maxX = curval;
}
curval = analog_GetYData();
if (curval < minY) {
minY = curval;
}
if (curval > maxY) {
maxY = curval;
}
curval = analog_GetZData();
if (curval < minZ) {
minZ = curval;
}
if (curval > maxZ) {
maxZ = curval;
}
}
}
/*while(!UART2IsEmpty())
UART2PutChar(UART2GetChar());*/
}
}
int main(void) {
int i, j, k, count;
unsigned char pat;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
dbprintf("\nLED Module Unit Test\n\n");
LED_Init(LED_BANK1 | LED_BANK2 | LED_BANK3);
for(count=0; count<10; count++) {
dbprintf("\nTesting LED_OnBank");
for(j=0;j<NUMLEDBANKS;j++) {
LED_OnBank((1<<j),0x0F);
}
DELAY();
dbprintf("\nTesting LED OffBank");
for(j=0;j<NUMLEDBANKS;j++) {
LED_OffBank((1<<j),0x0F);
}
DELAY();
LED_OnBank(LED_BANK1,0x0F);
DELAY();
LED_OffBank(LED_BANK1,0x0F);
LED_OnBank(LED_BANK2,0x0F);
DELAY();
LED_OffBank(LED_BANK2,0x0F);
LED_OnBank(LED_BANK3,0x0F);
DELAY();
LED_OffBank(LED_BANK3,0x0F);
LED_OnBank(LED_BANK2,0x0F);
DELAY();
LED_OffBank(LED_BANK2,0x0F);
LED_OnBank(LED_BANK1,0x0F);
DELAY();
LED_OffBank(LED_BANK1,0x0F);
DELAY();
LED_OnBank(LED_BANK1,0x01);
LED_OnBank(LED_BANK2,0x01);
LED_OnBank(LED_BANK3,0x01);
DELAY();
dbprintf("\nTesting LED_InvertBank");
for(k=0;k<3;k++) {
for(j=0;j<NUMLEDBANKS;j++) {
LED_InvertBank((1<<j),(0x03<<k));
}
DELAY();
}
for(k=0;k<3;k++) {
for(j=0;j<NUMLEDBANKS;j++) {
LED_InvertBank((1<<j),(0x0C>>k));
}
DELAY();
}
dbprintf("\nTesting LED_SetFullState");
LED_SetFullState(0x00);
DELAY();
for(j=0;j<12;j++) {
LED_SetFullState(1<<j);
DELAY();
}
for (j=0;j<12;j++) {
LED_SetFullState(0x800 >> j);
DELAY();
}
dbprintf("\nTesting LED_SetBank");
pat = 0x05;
for(k=0;k<4;k++) {
for(j=0;j<NUMLEDBANKS;j++) {
LED_SetBank((1<<j),pat);
pat ^= 0x0F;
}
DELAY();
}
}
LED_End();
return 0;
}
