void SetThree300K(uint8_t mode) {
_AN(THREE300K)=0;
_TRIS(THREE300K)=0;
if (mode==LOW) _LAT(THREE300K)=0;
if (mode==HIGH) _LAT(THREE300K)=1;
if (mode==IN) {_AN(THREE300K)=1; _TRIS(THREE300K)=1;}
}
void SetOne300K(uint8_t mode) {
_AN(ONE300K)=0;
_TRIS(ONE300K)=0;
if (mode==LOW) _LAT(ONE300K)=0;
if (mode==HIGH) _LAT(ONE300K)=1;
if (mode==IN) {_AN(ONE300K)=1; _TRIS(ONE300K)=1;}
}
void SetTwo300K(uint8_t mode) {
_AN(TWO300K)=0;
_TRIS(TWO300K)=0;
if (mode==LOW) _LAT(TWO300K)=0;
if (mode==HIGH) _LAT(TWO300K)=1;
if (mode==IN) {_AN(TWO300K)=1; _TRIS(TWO300K)=1;}
}
void DisableBluetooth() {
#ifndef WIN32
_LAT(BT_RESET) = 1;
_CNPUE(BT_RDYN_CN) = 0; // Disable pull-up
_LAT(BT_REQN) = 0;
#endif
}
void EnableBluetooth() {
#ifndef WIN32
_CNPUE(BT_RDYN_CN) = 1; // RDYN line must have a pull-up
_LAT(BT_RESET) = 0;
_LAT(BT_REQN) = 1;
#endif
}
void comms_set_led(byte led, byte value) {
switch (led) {
case 1:
_LAT(LED1) = value;
break;
case 2:
_LAT(LED2) = value;
break;
}
}
int main(void) {
InitializeSystem();
// Wait for reset button to be released
while (_PORT(PIO_BTN1) == HIGH) { }
//ADCInitialize();
PWMInitialize();
PWMEnable();
USBDeviceInit();
//ADCStartCapture();
_LAT(PIO_LED1) = HIGH;
ColourEngine::Initialize();
//ColourEngine::PowerOn(1000); // Fade in
ColourEngine::SetPower(power, 0);
//_LAT(PIO_LED2) = HIGH;
while (1) {
USBDeviceTasks();
USBUserProcess();
}
return 0;
}
void ColourEngine::Update() {
_LAT(PIO_LED2) = HIGH;
// Adjust current colour by brightness
RGBWColour colour = current_rgbw * current_brightness;
// Raw RGBW values are passed directly to the PWM outputs.
// Note that scaling by brightness will probably not look right, so refrain from using brightness control in this case!
// sRGBW values operate in a perceptually-uniform colour space
// ie, 0.5 = half brightness (not half power!)
// These values can be scaled while still appearing linear.
if (colour.space == RGBWColour::sRGB) {
colour = colour.to_linear();
}
PWMUpdate(colour.red, colour.green, colour.blue, colour.white);
_LAT(PIO_LED2) = LOW;
}
void ScreenOff() {
AppGlobalEvent(evtScreenOff, NULL);
accel_SetMode(accStandby);
// Disable drawing
draw_task->state = tsStop;
/*if (foreground_app != NULL) {
foreground_app->task->state = tsStop;
}*/
ssd1351_DisplayOff();
ssd1351_PowerOff();
_LAT(LED1) = 0;
_LAT(LED2) = 0;
displayOn = false;
}
int main (void)
{
unsigned char value;
//Configure all ports as inputs
TRISA = 0xFFFF; TRISB = 0xFFFF;// TRISC = 0xFFFF;
//---------------------------------------------------------------------
// OSCILATOR
//---------------------------------------------------------------------
oscilator_init();
//config_load();
//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
uart_init();
printf("Uart Init Ok\n");
//---------------------------------------------------------------------
// STATUS (LED/SOUND)
//---------------------------------------------------------------------
status_init();
// STATUS(BLINK_SLOW,_ON,_ON);
// __delay_ms(10);
// STATUS_INIT;
//---------------------------------------------------------------------
// ADC
//---------------------------------------------------------------------
// adc_init();
// timer_init_ms();
//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
i2c_init();
printf("i2c Init Ok\n");
ITG3200_begin();
printf("ITG3200 Ok\n");
ADXL345_begin();
printf("ADXL Init Ok\n");
//STATUS(BLINK_FAST,_ON,_ON);
printf("Init Gyro\n");
// STATUS_NORMAL;
//__delay_ms(100);
// Acceleromter calibration
int AcclCalibration;
AcclCalibration =1;
//#define AccCali
#ifdef AccCali
while(AcclCalibration){
int i;
double adjAccl;
for(i=0;i<10;i++){
ADXL345_update();
}
float Acclx=getAcclXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gravity=sqrt(Acclx*Acclx+Accly*Accly+Acclz*Acclz);
adjAccl= 1/Gravity;
ADXL345_Scaler =adjAccl * ADXL345_Scaler;
printf("%1.3f %1.3f %1.3f Gravity %1.9f y %1.9f %1.3f\n",Acclx,Accly,Acclz,Gravity,ADXL345_Scaler,adjAccl);
if (Gravity == 1.0000) AcclCalibration=0;
}
printf("Accelerometer Calibrated \n");
#endif
//#define GyroTest
#ifdef GyroTest
while(1){
ITG3200_update();
float GyroX=getGyroXOutput();
float GyroY=getGyroYOutput();
float GyroZ=getGyroZOutput();
int ID=getGyroIDOutput();
float Temp= getGyroTempOutput();
printf("%1.3f %1.3f %1.3f %1.3f %i \n",GyroX,GyroY,GyroZ,Temp,ID);
}
#endif
//---------------------------------------------------------------------
// MOTOR
//---------------------------------------------------------------------
motor_init();
printf("motor init ok\n");
__delay_ms(100);
//#define motor_debug
#ifdef motor_debug
//while(1){
// _LAT(pinMotor0) = 1;
// _LAT(pinMotor1) = 1;
// _LAT(pinMotor2) = 1;
// _LAT(pinMotor3) = 1;
//
// _TRIS(pinMotor0) = 0;
// _TRIS(pinMotor1) = 0;
// _TRIS(pinMotor2) = 0;
// _TRIS(pinMotor3)= 0;
//
//
//}
int i=0;
while(i<50){
_LAT(pinLed1) =! _LAT(pinLed1);
motor_set_duty(0,i);
motor_set_duty(1,i);
motor_set_duty(2,i);
motor_set_duty(3,i);
motor_apply_duty();
__delay_ms(50);
i = i + 1;
// if(i> 90) i = 0;
printf("Motor %x \n");
}
__delay_ms(250);
i=0;
motor_set_duty(0,i);
motor_apply_duty();
motor_set_duty(1,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(2,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(3,i);
__delay_ms(250);
motor_apply_duty();
__delay_ms(250);
#endif
//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();
//#define imu_debug
#ifdef imu_debug
//IMU debug comment out for production
float interval_ms=0;
while (1){
ITG3200_update();
ADXL345_update();
//interval_ms = timer_dt();
// if(interval_ms > 0 ){
//we have fresh adc samples
// printf("Time %lu ",interval_us);
imu_update(interval_ms);
float* K = dcmEst[2]; //K(body) vector from DCM matrix
float pitch_roll = acos(K[2]); //total pitch and roll, angle necessary to bring K to [0,0,1]
//cos(K,K0) = [Kx,Ky,Kz].[0,0,1] = Kz
float Kxy = sqrt(K[0]*K[0] + K[1]*K[1]);
float anglePitch = - (pitch_roll * asin(K[0]/Kxy))*50;//*180/PI ;
float angleRoll = (pitch_roll * asin(K[1]/Kxy))*50;//*180/PI;
//float angleRoll = (atan2(dcmEst[2][2],dcmEst[2][1]))*180/3.14;
//float anglePitch = -(asin(dcmEst[2][0]))*180/3.14;
// float angleYaw=(-atan2(dcmGyro[1][0],dcmGyro[0][0]))*180/3.14;
float driftX=(sin(anglePitch*(3.14/180))*(sin(angleRoll*(3.14/180))));
float CalcDriftX=getAcclXOutput()-driftX;
float Acclx=getAcclXOutput();
float Gyrox=getGyroXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gyroy=getGyroYOutput();
//printf("\n");
//if(0 == imu_sequence % 4){
// hdlc_send_byte(float_to_int(anglePitch));
// hdlc_send_byte(float_to_int(angleRoll));
// hdlc_send_byte(float_to_int(Acclx*100));
// hdlc_send_byte(float_to_int(Gyrox*1000));
// hdlc_send_byte(float_to_int(Accly*100));
// hdlc_send_byte(float_to_int(Gyroy*1000));
// //hdlc_send_byte(float_to_int(Acclz*100));
// //hdlc_send_byte(float_to_int(Kxy*100));
// //hdlc_send_byte(float_to_int(K[0]*100));
// //hdlc_send_byte(float_to_int(K[1]*100));
// //hdlc_send_byte(float_to_int(K[2]*100));
//
//// float GyroYpitch = GyroYpitch+ (getGyroYOutput()*(interval_us/1000));
//// printf("pitch %1.3f roll %1.3f Gyro ",pitch.value,roll.value,GyroYpitch);
//// printf(" Angle P%1.3f R%1.3f Drift=%1.3f Acclx=%1.3f Calc=%1.3f GyroX=%1.3f\n",anglePitch,angleRoll,driftX,Acclx,CalcDriftX,GyroX);
// hdlc_send_sep();
//}
}
}
void InitializeSystem() {
#ifdef BOARD_UBW32
// Disable ADC port (allows PORTB to be used for digital I/O)
AD1PCFG = 0xFFFF;
TRISE = 0x0000;
TRISB = 0x0000;
TRISC = 0x0000;
TRISD = 0x0000;
LATE = 0x0000;
LATB = 0x0000;
LATC = 0x0000;
LATD = 0x0000;
#endif
#ifdef BOARD_HEXLIGHT
ANSELA = 0x0000;
ANSELB = 0x0000;
#endif
LATA = 0x0000;
LATB = 0x0000;
// Ensure LED drivers are driven low as soon as possible
// _TRIS(PIO_OC1) = 0;
// _TRIS(PIO_OC2) = 0;
// _TRIS(PIO_OC3) = 0;
// _TRIS(PIO_OC4) = 0;
// _LAT(PIO_OC1) = OUTPUT;
// _LAT(PIO_OC2) = OUTPUT;
// _LAT(PIO_OC3) = OUTPUT;
// _LAT(PIO_OC4) = OUTPUT;
// Force disconnect of USB bootloader
U1CON = 0x00000000;
U1PWRC = 0x00000000;
// LEDs
// _TRIS(PIO_LED1) = OUTPUT;
// _TRIS(PIO_LED2) = OUTPUT;
#ifdef BOARD_UBW32
_TRIS(PIO_LED3) = OUTPUT;
_TRIS(PIO_LED_USB) = OUTPUT;
_TRIS(PIO_BTN_PGM) = 1;
_TRIS(PIO_BTN_USR) = 1;
#elif BOARD_HEXLIGHT
_TRIS(PIO_BTN1) = INPUT;
_TRIS(PIO_BTN2) = INPUT;
#endif
// _TRIS(PIO_USBP) = INPUT;
// _TRIS(PIO_USBN) = INPUT;
// _LAT(PIO_LED1) = LOW;
// _LAT(PIO_LED2) = LOW;
#ifdef BOARD_UBW32
_LAT(PIO_LED3) = HIGH;
_LAT(PIO_LED_USB) = LOW;
#endif
mJTAGPortEnable(0);
// Initializethe PIC32 core
//OSCConfig(OSC_POSC_PLL, OSC_PLL_MULT_20, OSC_PLL_POST_2, OSC_FRC_POST_2);
sys_clock = F_SYSCLK;
mOSCSetPBDIV(OSC_PB_DIV_1);
pb_clock = SYSTEMConfig(sys_clock, SYS_CFG_ALL);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
// Initialize core time base
SystickInit();
}
