void rotate_stepper2(unsigned long steps, unsigned char direction, unsigned long frequency, unsigned char mode){
if(mode){ // Full Stepper Mode
if(direction){ // ClockWise Direction in Full Stepper Mode
while(steps > 0){
if(pos_stepper2 > 3) pos_stepper2 = 0;
STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_full2[pos_stepper2++];
__delay32(frequency);
steps--;
}
}else{ // CounterClockWise Direction in Full Stepper Mode
while(steps > 0){
if(pos_stepper2 < 0) pos_stepper2 = 3;
STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_full2[pos_stepper2--];
__delay32(frequency);
steps--;
}
}
}else{ // Half Stepper Mode
if(direction){ // ClockWise Direction in Half Stepper Mode
while(steps > 0){
if(pos_stepper2 > 7) pos_stepper2 = 0;
STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_half2[pos_stepper2++];
__delay32(frequency);
steps--;
}
}else{ // CounterClockWise Direction in Half Stepper Mode
while(steps > 0){
if(pos_stepper2 < 0) pos_stepper2 = 7;
STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_half2[pos_stepper2--];
__delay32(frequency);
steps--;
}
}
}
}
/********************************************************************
* Function Name: sendnRFstring *
* Parameters: pointer to string, length of string. *
* Description: Writes a string to the transmit FIFO of nRF *
* and toggels the CE line to start *
* transmission. *
* *
********************************************************************/
void sendnRFstring(unsigned char *nRFstring, int size)
{
LDPageWriteSPI(0xA0, nRFstring, size);
__delay32(200); // Delay for >10us
_LATB15 = 1;
__delay32(200); // Delay for >10us
_LATB15 = 0;
}
void PWMEnable()
// TODO: metti un commento, please
{
// IDLE to 50% PWM
PDC1 = PDC2 = PDC3 = LOOPINTCY/2;
FLTACON = 0;
// L pwm out high
P1OVDCON = 0x0015;
// allows driver boost capacitors to charge
__delay32(24); // 8
//P1OVDCON = 0x0000;
// clear fault flag before start fault mechanism
IFS3bits.FLTA1IF = 0;
P1OVDCON = 0x3f00;
SysStatus.PWM_is_disabled = 0;
// Enable PWM generation
PTCONbits.PTEN = 1;
OverCurrentFaultIntEnable();
// PWM fault configuration
// pin pair 1,2 and 3 are controlled by Fault Input A
#ifdef ENABLE_OVER_CURRENT_PROTECTION
FLTACON = 7;
#else
FLTACON = 0; // fault disabled
#endif
}
void delay_ms(U32 u32ms)
{
U32 u32ticks;
u32ticks = ( ((U32)u32ms * 1000) / cTcy_ns) * 1000;
__delay32(u32ticks);
}
void delay_us(U32 u32us)
{
U32 u32ticks;
u32ticks = ( ((U32)u32us * 1000) / cTcy_ns);
__delay32(u32ticks);
}
void ADCDoOffsetCalibration(void)
// ADC Initialization:
// Performs offset calibration for ISENSES and initial base value acquisition for vdclink.
// Then setup ADC for for sequential sampling on CH0=AN0, CH1=AN1, CH2=AN2.
// without PWM sync and DMA, value and stored in MeasCurrParm.Offseta,b signed fractional form.
{
int i;
int ret;
int led;
// Configure ADC registers for calibration without PWM sync and DMA
ADCConfigureRegistersForCalibration();
// Execute calibration and store values
for(i=0;i<10000;i++){// delay 1s
__delay32(4000); //delay 100us
}
led = LED_status.RedBlinkRate;
while(1){
ret = ADCCalibrate() ;
if(-1 == ret){
/* AD is not working properly.
* loop forever if fatal error
* condition
*/
SysError.ADCCalFailure = 1;
LED_status.RedBlinkRate=BLINKRATE_STILL;
while(1);
}else if(-2 == ret){
/* VDCLink is too low.
* enlight red led, then
* wait forever for VDClink to
* raise
*/
SysError.ADCCalFailure = 1;
LED_status.RedBlinkRate=BLINKRATE_STILL;
// break;
continue;
}
LED_status.RedBlinkRate = led;
return;
}
}
int main() {
TRISBbits.TRISB7 = 0;
while(1) {
LATBbits.LATB7 = ~PORTBbits.RB7;
__delay32(1500000);
}
}
static void s_led_flash(void)
{
unsigned int i;
for(i=1;i<=20;i++)
{
LATAbits.LATA3 = ~LATAbits.LATA3;
__delay32(800000);
}
}
void ByteReadSPI2(unsigned char Add, unsigned char *rdptr, unsigned char length )
{
DS_CS_2 = 0; // Select Device
WriteSPI2( Add ); // WRITE word address to EEPROM
getsSPI2( rdptr, length ); // read in multiple bytes
DS_CS_2 = 1; // Deselect Device
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
uint8_t osd_spi_read(int8_t addr)
{
uint8_t SPIData;
OSD_CS = 0; // Set active-low CS low to start the SPI cycle
spi_write_raw_byte(addr); // Send the Address
__delay32(20000UL * OSD_SF);
SPIData = spi_xfer_raw_byte(0);
OSD_CS = 1; // Set active-low CS high to end the SPI cycle
return SPIData;
}
void ByteWriteSPI2(unsigned char Add, unsigned char Data )
{
DS_CS_2 = 0; // Select Device
WriteSPI2 ( Add ); // write address byte to EEPROM
WriteSPI2 ( Data ); // Write Byte to device
DS_CS_2 = 1; // Deselect device and initiate Write
SPI2STATbits.SPITBF = 0; // Transmit started, SPIxTXB is empty
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
int main (void)
{
while(OSCCONbits.LOCK!=1); /* Wait for PLL to lock */
__delay32(EEPROM_DELAY*10); /* Wait for EEPROMs to settle */
__delay32(30000000);
state = STATE_INIT;
while(1)
{
CLRWDT()
stateMachine(); /* call state machine */
}
}
/*
Figure out if there was a pulse without a trigger
The trigger pulse is a sample pulse so it comes in the middle of current pulse.
We should wait for 10us After this is entered. If there has not been a trigger pulse durring that time
then it was a false tirgger
*/
void __attribute__((interrupt, no_auto_psv)) _INT3Interrupt(void) {
// There was trigger on INT3
_INT3IF = 0;
__delay32(100); // wait for 10us
if (!global_data_A36582.sample_complete) {
// There was a current pulse without a sample trigger within the next 10us
global_data_A36582.pulse_with_no_trigger_counter++;
global_data_A36582.false_trigger_counter++;
ResetPulseLatches();
}
}
void lights(void){
LED1 = 0;
LED2 = 1;
LED3 = 1;
LED4 = 1;
__delay32(8000000);
LED1 = 1;
LED2 = 0;
__delay32(8000000);
LED2 = 1;
LED3 = 0;
__delay32(8000000);
LED3 = 1;
LED4 = 0;
__delay32(8000000);
LED4 = 1;
__delay32(2500000);
LED1 = !(resetStat & RST_POR);
LED2 = !(resetStat & RST_BOR);
LED3 = !(resetStat & RST_EXTR);
LED4 = !(resetStat & RST_CM);
__delay32(10000000);
LED1 = 1;
LED2 = 1;
LED3 = 1;
LED4 = 1;
}
int main(void) {
int duty = 500;
initialize();
write_duty(duty); //apply duty cycle change
motoron = 1;//turn motor on
kick();//kick start commutation
while (1){
commutate(2);
__delay32(40000000);
commutate(4);
__delay32(40000000);
commutate(3);
__delay32(40000000);
if(USER){
motoron=0;
}
LED1 = !S3;
LED2 = !S2;
LED3 = !S1;
//test_MayDay();
}
return 0;
}
/* The initialize function configures the PLL to set the internal clock
* frequency. It also configures the digital IO and calls the initialization
* functions for each of the modules. A light sequence signals the end of
* initialization.
*/
void initialize(void){
/* Configure Phase Lock Loop for the system clock reference at 40MHz */
// Fosc (Clock frequency) is set at 80MHz
// Fin is 7.37 MHz from internal FRC oscillator
// FPLLI = Fin/N1 = 3.685 MHz
CLKDIVbits.PLLPRE = 0; // N1 = 2
// FVCO = FPLLI*M1 = 162.14MHz
PLLFBDbits.PLLDIV = 42; // M = 44
// FPLLO = FVCO/N2 = 81.07 MHz
// FOSC ~= 80MHz, FCY ~= 40MHz
CLKDIVbits.PLLPOST = 0; // N2 = 2
/* Initiate Clock Switch */
//The __builtin macro handles unlocking the OSCCON register
__builtin_write_OSCCONH(1); //New oscillator is FRC with PLL
__builtin_write_OSCCONL(OSCCON | 0x01); //Enable clock switch
while (OSCCONbits.COSC!= 1); //Wait for FRC with PLL to be clock source
while (OSCCONbits.LOCK!= 1); //Wait for PLL to lock
/* Configure IO*/
TRISDbits.TRISD10 = 1; //USER input
//LED outputs
ANSELBbits.ANSB13 = 0; //Disable Analog on B13
TRISBbits.TRISB13 = 0; //LED1
ANSELBbits.ANSB12 = 0; //Disable Analog on B12
TRISBbits.TRISB12 = 0; //LED2
TRISDbits.TRISD11 = 0; //LED3
TRISDbits.TRISD0 = 0; //LED4
//Magnet Control
TRISBbits.TRISB14 = 0; //Top Magnet
//Store bits indicating reason for reset
resetStat = RCON;
//Clear reset buffer so next reset reading is correct
RCON = 0;
/* Initialize peripherals*/
initialize_PWM();
initialize_CN();
initialize_ADC();
initialize_QEI();
initialize_UART();
initialize_UART2();
//initialize_I2C_Master();
lights();
__delay32(10000000);
//initialize_MPU();
initialize_encoder_values(1600,1700,1800);
}
int main(void)
{
if (!IS_MCU_PROGRAMMED()) /* Stay in programming */
__delay32((unsigned long)((1000)*(FCY_UP)/1000ULL));
mcu_id = MCU_ID;
if (isPOWER_ON(rst_state = get_reset_state()))
power_on_init(); /* For POR, BOR and MCLR */
rst_events |= rst_state; /* As RCON register */
++rst_num; /* Calculate session reset number */
check_traps(); /* Do some possible traps */
/* SIM doesn't clear SRbits.IPL, ICD2 clears it */
SET_CPU_IPL(MAIN_IPL); /* Set default by hands */
clr_reset_state(); // Clear uParam and RCON
for(;;);
return(0); /* Never return */
}
//ISR for CMP3
void __attribute__((__interrupt__, __auto_psv__)) _CMP3Interrupt(void){
//Normal Mode
if (sampling_mode==0){ //Normal Mode
int temp=0;
//putHex(65);
ADCPC0bits.SWTRG0 = 1; //start conversion of AN0 and AN1 store 200 samples each
while(temp<400){
while(ADCPC0bits.PEND0){} //conv pending becomes 0 when conv complete
AN[temp]=ADCBUF0>>2;
temp++;
AN[temp]=ADCBUF1>>2;
temp++;
__delay32(delay_cycles);
ADCPC0bits.SWTRG0 = 1; //start conversion of AN0 and AN1
// sets PEND0 to 1
}
busy=0;
IEC1bits.AC3IE =0; //Disable interrupt
//IFS1bits.AC3IF =0; //clear interrupt flag=0
}
void BurstWriteSPI2(unsigned char Address, unsigned char data0, unsigned char data1,
unsigned char data2, unsigned char data3,
unsigned char data4, unsigned char data5,
unsigned char data6, unsigned char data7)
{
DS_CS_2 = 0; // Select Device
WriteSPI2 ( Address ); // write address byte to EEPROM
WriteSPI2 ( data0 ); // Write Byte to device
WriteSPI2 ( data1 ); // Write Byte to device
WriteSPI2 ( data2 ); // Write Byte to device
WriteSPI2 ( data3 ); // Write Byte to device
WriteSPI2 ( data4 ); // Write Byte to device
WriteSPI2 ( data5 ); // Write Byte to device
WriteSPI2 ( data6 ); // Write Byte to device
WriteSPI2 ( data7 ); // Write Byte to device
DS_CS_2 = 1; // Deselect device and initiate Write
SPI2STATbits.SPITBF = 0;
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
int main()
{
int i = 0;
_SWDTEN = 0; // Disable watchdog timer
_GIE = 0; // Disable interrupts
ANSELB = ANSELC = ANSELD = ANSELE = ANSELG = 0;
__delay32(8000000/2); // Wait about 500ms before change to PLL
Sys_Init();
LED1_W = 1; LED2_W = 1; LED3_W = 1; LED4_W = 1; // turn off
LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 1000);
LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 1000);
// LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 2000);
// LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 2000);
// LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 3000);
// LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 3000);
LED1_W = 1; LED2_W = 1; LED3_W = 1; LED4_W = 1; // turn off
while(1)
{
WR_Handle_Comms();
if(led)
{
i++;
//if (rx_data == 200)
//{
LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 100);
LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 100);
//}
//led = 0;
}
}
return 0;
}
void __attribute__((__interrupt__(__preprologue__("BCLR ADCON1, #1")), no_auto_psv)) _INT1Interrupt(void) {
/*
A sample trigger has been received
*/
// DPARKER these functions may mess with the timing
global_data_A36582.sample_energy_mode = ETMCanSlaveGetPulseLevel();
global_data_A36582.sample_index = ETMCanSlaveGetPulseCount();
global_data_A36582.sample_complete = 1;
// Check that there was enough time between pulses
//
if ((TMR2 <= MINIMUM_PULSE_PERIOD_T2) && (_T2IF == 0)) {
global_data_A36582.minimum_pulse_period_fault_count++;
}
_T2IF = 0;
TMR2 = 0;
// Wait for the pulse energy to dissipate
__delay32(150);
// Noise from the PFN will often trigger the pic interrupt input
// This checks that the interrupt trigger was a real trigger pulse
if (_RA12 == 0) {
global_data_A36582.pulse_with_no_trigger_counter++;
global_data_A36582.false_trigger_counter++;
}
// Read the data from port
PIN_OUT_TP_F = 1;
PIN_ADC_CHIP_SELECT = OLL_ADC_SELECT_CHIP;
global_data_A36582.imag_external_adc.filtered_adc_reading = SendAndReceiveSPI(0, ETM_SPI_PORT_2);
PIN_ADC_CHIP_SELECT = !OLL_ADC_SELECT_CHIP;
PIN_OUT_TP_F = 0;
_INT1IF = 0;
}
void CalibrateGyro(void)
{
int i;
int ii;
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
AngleOffset[0] = 0;
AngleOffset[1] = 0;
for (i=0;i<1000;i++)
{
/** Read sensors ******************************************************/
readSensorData();
__delay32(5000); // Without this delay, the I2C command acts funny...
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
for (ii=0;ii<3;ii++)
{
GyroOffset[ii] = GyroOffset[ii] + gyro[ii];
}
AngleOffset[0] += accXangle;
AngleOffset[1] += accYangle;
}
for (ii=0;ii<3;ii++)
{
GyroOffset[ii] = GyroOffset[ii]/1000;
}
AngleOffset[0] = AngleOffset[0]/1000.0;
AngleOffset[1] = AngleOffset[1]/1000.0;
gyroXangle = AngleOffset[0];
gyroYangle = AngleOffset[1];
compAngleX = AngleOffset[0];
compAngleY = AngleOffset[1];
}
void AllOfTheLights(void){
LED1 = 0;
__delay32(8000000);
LED1 = 1;
LED2 = 0;
__delay32(8000000);
LED2 = 1;
LED3 = 0;
__delay32(8000000);
LED3 = 1;
LED4 = 0;
__delay32(8000000);
LED4 = 1;
__delay32(2500000);
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
__delay32(10000000);
LED1 = 1;
LED2 = 1;
LED3 = 1;
LED4 = 1;
}
void ResetPulseLatches(void) {
PIN_PULSE_LATCH_RESET = OLL_RESET_LATCHES;
__delay32(20);
PIN_PULSE_LATCH_RESET = !OLL_RESET_LATCHES;
}
/* CS SENZA MOS DOPO ISO -> CS_H = 1 e CS_L = 0
CS CON MOS DOPO ISO -> CS_H = 0 e CS_L = 1 */
void __attribute__((__interrupt__,__auto_psv__)) _DMA4Interrupt(void) // DMA4 SPI
{
_DMA4IF = 0;
unsigned int static tmp __attribute__((space(dma),aligned(2)));
int spi_temp;
unsigned int cl=0;
switch (StatoSPI)
{
case 0: /* Idle */
break;
case 1: /* Start a new SPI Sequence */
SPI2STATbits.SPIROV = 0;
spi_temp = SPI2BUF;
/* Toggle pin */
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
__delay32(4074); /* Delay */
PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */
__delay32(4000); /* Delay */
/* Write only Sequence */
DMA5CNT = 3; /* DMA (SPI) Tx */ //Num byte + 1
DMA5STA = (unsigned int) &SPI1_Buff16[0];
DMA4CNT = 3; /* DMA (SPI) Rx */
DMA4STA = (unsigned int) &tmp;
DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */
DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */
DMA4CON &= 0xF7FF; /* Disable Null data Write */
DMA5CONbits.CHEN = 1; /* Enable DMA Tx */
DMA4CONbits.CHEN = 1; /* Enable DMA Rx */
DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */
for (cl=0; cl<4000; cl++);
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
SPI_WRITING = 0;
break;
case 2: //READ TX
/* Toggle pin */
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
__delay32(5074); /* Delay */
PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */
__delay32(5000);
SPI2STATbits.SPIROV = 0;
spi_temp = SPI2BUF;
/* Write only Sequence */
DMA5CNT = 2; /* DMA (SPI) Tx */
DMA5STA = (unsigned int) &data_req[0];
DMA4CNT = 2; /* DMA (SPI) Rx */
DMA4STA = (unsigned int) &tmp;
DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */
DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */
DMA4CON &= 0xF7FF; /* Disable Null data Write */
DMA5CONbits.CHEN = 1; /* Enable DMA Tx */
DMA4CONbits.CHEN = 1; /* Enable DMA Rx */
DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */
for (cl=0; cl<5000; cl++);
SPI2STATbits.SPIROV = 0;
spi_temp = SPI2BUF;
DMA4STA = (unsigned int) &data_rx[0];
DMA4CNT = read_size; /* DMA (SPI) Rx */
DMA4CON &= 0xFFEF; /* Use DMA Rx Post-Incrementation */
DMA4CON |= 0x800; /* Null data Write in addition do Peripheral to RAM write */
DMA4CONbits.CHEN = 1; /* Enable DMA Rx */
SPI2BUF = 0; /* Start DMA sequence */
for (cl=0; cl<18000; cl++);
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
break;
case 3: /* Start a new SPI Sequence */
for(cicli_dma=0; cicli_dma <= cicli_init+1; cicli_dma+=4){
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
/* Delay */
__delay32(4074); /* Delay */
/* Toggle pin */
PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */
__delay32(4000);
SPI1_Buff16[0] = VettoreInit[cicli_dma];
SPI1_Buff16[1] = VettoreInit[cicli_dma+1];
SPI1_Buff16[2] = VettoreInit[cicli_dma+2];
SPI1_Buff16[3] = VettoreInit[cicli_dma+3];
SPI2STATbits.SPIROV = 0;
/* Write only Sequence */
DMA5CNT = 3; /* DMA (SPI) Tx */
DMA5STA = (unsigned int) &SPI1_Buff16[0];
DMA4CNT = 3; /* DMA (SPI) Rx */
DMA4STA = (unsigned int) &tmp;
DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */
DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */
DMA4CON &= 0xF7FF; /* Disable Null data Write */
DMA5CONbits.CHEN = 1; /* Enable DMA Tx */
DMA4CONbits.CHEN = 1; /* Enable DMA Rx */
DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */
for (cl=0; cl<12000; cl++);
PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */
for (cl=0; cl<20000; cl++);
}
break;
default: /* Sequence finished */
StatoSPI = 0; /* Should never happend */
break;
} /* End of switch case sequence*/
IFS2bits.DMA4IF = 0; // Clear the DMA0 interrupt flag bit
} /* End of interrupt */
void delay_ms(unsigned int N)
{
__delay32(FCY/1000*N);
}
void RCInitCypressCYRF6936(void)
{
unsigned char Address; //Low Order Address Byte
unsigned char Data=0; //Data Byte
unsigned char Length;
unsigned char PageString[128];
#if 0
RC_reset = 1; // Reset the Cypress
__delay32(MILLISEC); // 1 milisec delay
RC_reset = 0; // Reset the Cypress
__delay32(MILLISEC); // 1 milisec delay
#endif
Address = 0x81; // TX_LENGTH_ADR
Data = 0x10;
ByteWriteSPI2(Address, Data);
Address = 0x82; // TX_CTRL_ADR -> TX disabled
Data = 0x00;
ByteWriteSPI2(Address, Data);
Address = 0x83; // TX_CFG_ADR
Data = 0x2F;
ByteWriteSPI2(Address, Data);
Address = 0x86; // RX_CFG_ADR
Data = 0x4A;
ByteWriteSPI2(Address, Data);
Address = 0x87; // RX_IRQ_STATUS_ADR
Data = 0x00;
ByteWriteSPI2(Address, Data);
Address = 0x8B; // PWR_CTRL_ADR
Data = 0x00;
ByteWriteSPI2(Address, Data);
Address = 0x8C; // XTAL_CTRL_ADR
Data = 0x04;
ByteWriteSPI2(Address, Data);
Address = 0x8D; // IO_CFG_ADR
Data = 0x41;
ByteWriteSPI2(Address, Data);
Address = 0x8E; // GPIO_CTRL_ADR
Data = 0x00;
ByteWriteSPI2(Address, Data);
Address = 0x90; // FRAMING_CFG_ADR
Data = 0xEE;
ByteWriteSPI2(Address, Data);
Address = 0x9B; // TX_OFFSET_LSB_ADR
Data = 0x55;
ByteWriteSPI2(Address, Data);
Address = 0x9C; // TX_OFFSET_MSB_ADR
Data = 0x05;
ByteWriteSPI2(Address, Data);
Address = 0x9D; // MODE_OVERRIDE_ADR
Data = 0x18;
ByteWriteSPI2(Address, Data);
Address = 0xB2; // AUTO_CAL_TIME
Data = 0x3C;
ByteWriteSPI2(Address, Data);
Address = 0xB5; // AUTO_CAL_OFFSET
Data = 0x14;
ByteWriteSPI2(Address, Data);
Address = 0x9E; // RX_OVERRIDE_ADR
Data = 0x90;
ByteWriteSPI2(Address, Data);
Address = 0x9F; // TX_OVERRIDE_ADR
Data = 0x00;
ByteWriteSPI2(Address, Data);
Address = 0x8F; // XACT_CFG_ADR
Data = 0x2C;
ByteWriteSPI2(Address, Data);
Address = 0xA8; // CLK_EN_ADR
Data = 0x02;
ByteWriteSPI2(Address, Data);
Address = 0xA7; // CLK_OVERRIDE_ADR
Data = 0x02;
ByteWriteSPI2(Address, Data);
Address = 0xA2; // SOP_CODE_ADR
BurstWriteSPI2(Address, 0xDF, 0xB1, 0xC0, 0x49, 0x62, 0xDF, 0xC1, 0x49);
Address = 0x8F; // XACT_CFG_ADR -> clr FRC END
Data = 0x0C;
ByteWriteSPI2(Address, Data);
Address = 0x41; // TX_LENGTH_ADR
Length = 16;
ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read
Address = 0x5D;
Length = 3;
ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read
Address = 0x66;
Length = 3;
ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read
Address = 0x72;
Length = 1;
ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read
Address = 0x75;
Length = 1;
ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read
ERROR_Level1_COUNTER=0;
ERROR_Level2_COUNTER=0;
}
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
unsigned int *chptr; // For sending a float value
unsigned int command; // For sending commands to Axis Converter
double gyroXrate, gyroYrate;
char nRFstatus;
unsigned char rfCommands[32]; // Commands received from remote
unsigned char nRFregisters[10];
int iii;
long long xSpeedFilterSum = 0;
long long ySpeedFilterSum = 0;
// Toggle LED
_LATA4 = 1;
/** Read sensors ******************************************************/
readSensorData();
__delay32(1000); // Without this delay, the I2C command acts funny...
accXangle = ((atan2(accel[0], accel[2]))*RAD_TO_DEG)-AngleOffset[0];
accYangle = ((atan2(accel[1], accel[2]))*RAD_TO_DEG)-AngleOffset[1];
gyroXrate = ((double)gyro[0]-(double)GyroOffset[0])/131;
gyroYrate = -(((double)gyro[1]-(double)GyroOffset[1])/131);
/** Feedback Loop *****************************************************/
compAngleX = (compFilterGyro*(compAngleX+(gyroYrate*(double)SAMPLE_TIME)))+(compFilterAccel*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (compFilterGyro*(compAngleY+(gyroXrate*(double)SAMPLE_TIME)))+(compFilterAccel*accYangle);
if (enableSteppers == 1)
{
iCompAngleY += compAngleY;
iCompAngleX += compAngleX;
}
else
{
iCompAngleY = 0.0;
iCompAngleX = 0.0;
}
ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN+gyroXrate*(double)iGAIN);
xSpeed = -((compAngleX-sGAIN)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN+gyroYrate*(double)iGAIN);
xSpeedFilter[SpeedFilterCounter] = xSpeed;
ySpeedFilter[SpeedFilterCounter] = ySpeed;
for(iii = 0; iii<=SpeedFilterLength;iii++)
{
xSpeedFilterSum += xSpeedFilter[iii];
ySpeedFilterSum += ySpeedFilter[iii];
}
xSpeed = (long long)xSpeedFilterSum/(SpeedFilterLength+1);
ySpeed = (long long)ySpeedFilterSum/(SpeedFilterLength+1);
// ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN);
// xSpeed = -((compAngleX)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN);
// if (xSpeed<0)
// xSpeed = -(xSpeed*xSpeed);
// else
// xSpeed = xSpeed*xSpeed;
// if (ySpeed<0)
// ySpeed = -(ySpeed*ySpeed);
// else
// ySpeed = ySpeed*ySpeed;
compAngleXlast = compAngleX;
compAngleYlast = compAngleY;
// xSpeed = gyroXrate *1000;
// ySpeed = gyroYrate *1000;
/** Build command string **********************************************/
command = 0b1000000000000000;
if (enableSteppers == 1)
command = command | 0b0100000000000000;
/* Send data to Axis Controller ***************************************/
chptr = (unsigned char *) ∠ // For sending a float value
CS_Axis = 0;
/* Command: MSB
* 15: 0 = NO Speed Values
* 14: 1 = Enable Stepper Driver
* 13:
*/
WriteSPI2(command); // Command: MSB
WriteSPI2(xSpeed); // Send x-velocity vector
WriteSPI2(ySpeed); // Send y-velocity vector
WriteSPI2(*chptr++); // Sending first part of float value
WriteSPI2(*chptr); // Sending second part of float value
CS_Axis = 1;
/** Prepare telemetry *************************************************/
setRawData2string();
/**** Telemetry string format for command=0x42 ************************/
/** command, axh, axl, ayh, ayl, azh, azl, th, tl, 9byte **/
/** gxh, gxl, gyh, gyl, gzh, gzl, cxh, cxl, cyh, cyl, czh, czl,12byte**/
/** motorStatus 1byte**/
/** --Total 22byte **/
serString[16] = xSpeed & 0xFF; // Temporary sending additional telemetry
serString[15] = xSpeed >> 8;
serString[18] = ySpeed & 0xFF;
serString[17] = ySpeed >> 8;
// serString[18] = iax & 0xFF;
// serString[17] = iax >> 8;
// serString[20] = igy & 0xFF;
// serString[19] = igy >> 8;
serString[21] = enableSteppers;
SpeedFilterCounter++;
if (SpeedFilterCounter> SpeedFilterLength)
SpeedFilterCounter = 0;
/** Receive RF commands if any ********************************************/
// LDByteWriteSPI(0x20, 0b00001010);
nRFstatus = LDBytePollnRF();
for(iii=0; iii<10;iii++) // For debug
{
nRFregisters[iii] = readnRFbyte(iii);
}
if (nRFstatus & 0b01000000) // Data Ready RX FIFO
{
// _LATB4 = 1;
//// Delay10TCYx(100);
LDByteReadSPI(0x61, rfCommands, 21); // Read RX FIFO
LDByteWriteSPI(0x27 , 0b01000000); // Clear RX FIFO interrupt bit
//ByteWriteSPI(0xE2 , 0xFF); //Flush RX FIFO
if(rfCommands[0] == 0x82)
{
if(enableSteppers)
{
enableSteppers = 0;
buttonState = 0;
//_LATB4 = 0;
} else {
enableSteppers = 1;
buttonState = 2;
//_LATB4 = 1;
}
} else if (rfCommands[0] == 0x83)
{
_LATB4 = 1;
// Set the received values to variables
for(iii=1;iii<19;iii += 3){
if(rfCommands[iii]<9)
setValueCode[rfCommands[iii]] = (rfCommands[iii+1]<<8) + rfCommands[iii+2];
}
pGAIN = setValueCode[1];
dGAIN = setValueCode[2];
compFilterGyro = (float)setValueCode[4]/(float)1000;
compFilterAccel = 1.0 - compFilterGyro;
iGAIN = (float)setValueCode[3]/100.0;
//angle = (setValueCode[6]-180)* PI /180.0;
//SpeedFilterLength = setValueCode[5];
LDByteWriteI2C(0x00D0, 0x1A, setValueCode[5]); // 0.0 ms and disable FSYNC
sGAIN = (float)(setValueCode[6]-1000)/100.0;
}
// //LATDbits.LATD3 = 0;
//
}
// LDByteWriteSPI(0x20, 0b00001010); //
// LDByteWriteSPI(0x26, 0b00000010); // Set RC_CH to reset retransmit counter
LDCommandWriteSPI(0xE2); // Flush RX buffer (not during ACK)
/** Send telemetry ********************************************************/
// _LATB15 = 0; // Set CE low to stop receiving data
// LDByteWriteSPI(0x20 , 0b00001010); // Set to send mode
// __delay32(100);
LDByteWriteSPI(0x27, 0b00010000); // clear MAX_RT (max retries)
LDCommandWriteSPI(0xE1); // Flush TX buffer (tx buffer contains last failed transmission)
LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
sendnRFstring( serString, 22);
/** Set nRF to recive mode ************************************************/
// __delay32(1000);
// LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
// LDByteWriteSPI(0x20 , 0b00001011); // Set to receive mode
// _LATB15 = 1; // Set CE high to start receiving data
/* Done with interrupt ****************************************************/
_LATA4 = 0;
_T1IF = 0; // Clear Timer 1 interrupt flag
}
int16_t main(void)
{
unsigned int buttonCounter = 20000;
int i;
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
//initSerial();
initSPI1();
initSPI2();
InitI2C();
__delay32(1600000); // allow for POR for all devices
initnRF();
ControlByte = 0x00D0; // mpu6050 address
InitMPU6050(ControlByte);
// InitHMC5883L();
__delay_ms(500);
/** Init control loop *****************************************************/
readSensorData();
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
AngleOffset[0] = accXangle;
AngleOffset[1] = accYangle;
CalibrateGyro(); // Finding the gyro zero-offset
SetupInterrupts();
////// int serStringN = 14;
////// char nRFstatus = 0;
//////// int i;
////// delaytime = 1;
////// bool mode = 0;
while(1)
{
/** Read Button RB7 for enable steppers *******************************/
if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 1;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter)
{
enableSteppers = 2;
__delay32(40000000);
buttonState = 2;
buttonCounter = 20000;
enableSteppers = 1;
}
if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 3;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter)
{
buttonState = 0;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonCounter)
{
buttonCounter--;
}
for (i=0;i<100;i++)
{
i=i;
}
// __delay32(100);
}
}
int ADCCalibrate(void)
// Calculate mean value of ADC_CAL_N_SAMPLES samples for iA, iB and Vdc and store in
// MeasCurrParm.Offseta, Offsetb
// DMA AND ADC isr are not enabled here!
// An error in estimation of current offset results in a cogging torque when
// torque set point is 0; the value can be adjusted looking at the 3 phase ia, ib, and ic
// and centering all values around 0
{
int i,TOT;
long ADCVdcZero = 0;
long ADCOffs1 = 0, ADCOffs2 = 0;
// Put an 'accettable' value in case of error in the calibration process
MeasCurrParm.Offseta = 0x0;
MeasCurrParm.Offsetbc = 0x0;
// Turn on ADC module
AD1CON1bits.ADON = 1;
for(i=0;i<ADC_CAL_N_SAMPLES;i++)
{
// samples AN0
AD1CHS0bits.CH0SA = 0;
AD1CON1bits.DONE = 0;
// amplifiers holding
AD1CON1bits.SAMP = 1;
// TODO: TRIM ME (eventually:)
__delay32(100);
// convert!
AD1CON1bits.SAMP = 0;
// init time out timer
TOT=0;
// poll for end of conversion
while( 0 == AD1CON1bits.DONE)
{
// Avoids polling lock
if (ADC_CAL_TIMEOUT == ++TOT)
{
// offset correction error signaling
return -1;
}
}
// accumulate values
ADCOffs1 += (int)ADC1BUF0;
// Samples AN1
AD1CHS0bits.CH0SA = 1;
AD1CON1bits.DONE = 0;
// amplifiers holding
AD1CON1bits.SAMP = 1;
// TODO TRIM ME
__delay32(100);
// convert!
AD1CON1bits.SAMP = 0;
// init time out timer
TOT=0;
// poll for end of conversion
while( 0 == AD1CON1bits.DONE )
{
// Avoids polling lock
if (ADC_CAL_TIMEOUT == ++TOT)
{
// offset correction error signaling
return -1;
}
}
// accumuulate values
ADCOffs2 += (int)ADC1BUF0;
// Samples AN2 (Vdc)
AD1CHS0bits.CH0SA = 2;
AD1CON1bits.DONE = 0;
// amplifiers holding
AD1CON1bits.SAMP = 1;
// TODO TRIM ME */
__delay32(100);
// convert!
AD1CON1bits.SAMP = 0;
// don't mind about int generated
// init time out timer
TOT=0;
// poll for end of conversion
while( 0 == AD1CON1bits.DONE )
{
// Avoids polling lock
if (ADC_CAL_TIMEOUT == ++TOT)
{
// offset correction error signaling
return -1;
}
}
// accumuulate values
ADCVdcZero += (int)ADC1BUF0;
/* if the measured VDCLINK is lower than this threshold
* then we cannot be sure the Allegro are properly powered.
* For this reason the ADC calibration is not reliable.
*/
if(ADCVDCLinkTo100mV(ADC1BUF0) < ADC_VDCLINK_ALLEGRO_MIN_THRESHOLD)
return -2;
}
// divide for number of samples in order to get mean
ADCOffs1 /= ADC_CAL_N_SAMPLES;
ADCOffs2 /= ADC_CAL_N_SAMPLES;
ADCVdcZero /= ADC_CAL_N_SAMPLES;
// associate the calculated offset
MeasCurrParm.Offseta = ADCOffs1;
MeasCurrParm.Offsetbc = ADCOffs2;
// TODO: mettere a posto la correzione dell'offset
// e quindi togliere questi bypass!
//MeasCurrParm.Offseta = 0x200;
//MeasCurrParm.Offsetbc = 0x200;
// TODO: un commentino, no?
TargetDCbus = ADCVdcZero;
// Turn off ADC module
AD1CON1bits.ADON = 0;
return 0;
}
