/**************************************************************************** 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(); }
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; }
int main(void) { //DEVCFG1bits.FCKSM = 0; int c = 0; // for (c = 0; c < 100000; c++) { // Nop(); // } BOARD_Init(); //INTEnableSystemMultiVectoredInt(); //SERIAL_Init(); //DEVCFG1bits.FCKSM //TIMERS_Init(); // SYSKEY = 0; // SYSKEY = 0xAA996655; // SYSKEY = 0x556699AA; // OSCCONbits.FRCDIV = 0b111; // OSCConfig(OSC_FRC_PLL, OSC_PLL_MULT_20, OSC_PLL_POST_1, OSC_PB_DIV_1); LED_Init(LED_BANK1); LED_SetBank(LED_BANK1,~2); //while(1); //SERIAL_Init(); //LATBbits.LATB7 = 1; TIMERS_Init(); unsigned char count=0; //while (1) { // LATCbits.LATC3 ^= 1; //} printf("Hello World\r\n"); while (1) { if (!IsTimerActive(0)) { // LATCbits.LATC5 ^= 1; //LATBbits.LATB9 ^= 1; InitTimer(0, 500); printf("TOP: %X\r\n",count); LED_SetBank(LED_BANK1,count); count++; } if (!IsReceiveEmpty()) { //LATBbits.LATB8 ^= 1; printf("%c", GetChar()); } } while (1); }
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; }
void init_main() { /*Initialize modules*/ timer_init(); led_init(); /*Initialize peripherals*/ USART_Init(1); TIMERS_Init(); EEPROM_Init(); ws2812Init(); ledStringInit(); ledEffectInit(); adcInit(); ExtIntInit(); ISR_init(); }
void IMU_Init(void) { TIMERS_Init(); mympu_open(20); /* my version of initialization // reset the device MPU6500_writeOneByte( MPU6500_RA_PWR_MGMT_1, 0x80); InitTimer( 1, 100 ); while (IsTimerActive(1)); // reset gyro, accel, temp MPU6500_writeOneByte( MPU6500_RA_SIGNAL_PATH_RESET, 0x07); InitTimer( 1, 100 ); while (IsTimerActive(1)); uint8_t userCtrl = 0x10 | 0x8 | 0x4 | 0x1; MPU6500_writeOneByte( MPU6500_RA_USER_CTRL, userCtrl ); #ifdef DEBUG uint8_t tmp = MPU6500_readOneByte(MPU6500_RA_USER_CTRL); printf("user control should be %0x: %0x\n", userCtrl, tmp); #endif InitTimer( 1, 1000 ); while (IsTimerActive(1)); MPU6500_writeOneByte( MPU6500_RA_CONFIG, 0x01); uint8_t fifoEnConfig = MPU6500_FIFO_EN_ACC | MPU6500_FIFO_EN_GYRO; MPU6500_writeOneByte( MPU6500_RA_FIFO_EN, fifoEnConfig); #ifdef DEBUG tmp = MPU6500_readOneByte(MPU6500_RA_FIFO_EN); printf("fifo_en should be %0x: %0x\n", fifoEnConfig, tmp); #endif MPU6500_writeOneByte( MPU6500_RA_GYRO_CONFIG, 0b011000); // 2k dps #ifdef DEBUG tmp = MPU6500_readOneByte(MPU6500_RA_GYRO_CONFIG); printf("gyro_config should be %0x: %0x\n", 0b011000, tmp); #endif MPU6500_writeOneByte( MPU6500_RA_USER_CTRL, 0x00 | 0x40 | 0x10 ); //*/ return; }
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; BOARD_Init(); printf("Anima Small Scale FreeScale Accelerometer test\r\n"); TIMERS_Init(); LED_Init(LED_BANK1); int count=0; // while (1) { // if (!IsTimerActive(0)) { // // LATCbits.LATC5 ^= 1; // //LATBbits.LATB9 ^= 1; // InitTimer(0, 500); // printf("TOP: %X\r\n", count); // LED_SetBank(LED_BANK1, count); // count++; // // } // } // // while (1); free_accel_init(); printf("Scale is set to %d \r\n", free_GetScale()); free_SetScale(FREE_8GSCALE); printf("Scale is set to %d and should be %d\r\n", free_GetScale(), FREE_4GSCALE); free_SetScale(FREE_2GSCALE); printf("Scale is set to %d and should be %d\r\n", free_GetScale(), FREE_2GSCALE); printf("Rate is set to %d \r\n", free_GetRate()); free_SetRate(FREE_200HERTZ); printf("Rate is set to %d and should be %d\r\n", free_GetRate(), FREE_200HERTZ); free_SetRate(FREE_800HERTZ); printf("Rate is set to %d and should be %d\r\n", free_GetRate(), FREE_800HERTZ); //free_SetRate(FREE_1P56HERTZ); //while(1); char humanread = 1; short dataArray[3]; int i, j, data; for (i = 0; i != 1000; i++) for (j = 0; j != 500; j++) Nop(); while (1) { //if (UART2HalfEmpty()){ if (humanread == 1) { if (IsTransmitEmpty()) { free_GetTriplet(dataArray); printf("Accel: Cur X: %d \tCur Y: %d \tCur Z: %d\r\n", dataArray[0], dataArray[1], dataArray[2]); } } else { data = free_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(); 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());*/ } }