Part Number: TMS320F28335
Tool/software: Code Composer Studio
Only I2C reg does not work!!! Pls help me..
/* * main.c */ #include "DSP28x_Project.h" // Device Headerfile and Examples Include File #include <math.h> #include "DSP2833x_EQep.h" //제어 주기 #define Control_period 0.01 //motor+ //motor+ //#define motorDir1Set GpioDataRegs.GPBSET.bit.GPIO60 = 1 pin 안돼서 68로 바꿈 //#define motorDir1Clear GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1 #define motorDir1Set GpioDataRegs.GPCSET.bit.GPIO68 = 1 //오른똑 #define motorDir1Clear GpioDataRegs.GPCCLEAR.bit.GPIO68 = 1 #define motorDir2Set GpioDataRegs.GPBSET.bit.GPIO61 = 1 #define motorDir2Clear GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1 #define motorDir5Set GpioDataRegs.GPCSET.bit.GPIO64 = 1 //왼쪽 #define motorDir5Clear GpioDataRegs.GPCCLEAR.bit.GPIO64 = 1 #define motorDir6Set GpioDataRegs.GPCSET.bit.GPIO65 = 1 #define motorDir6Clear GpioDataRegs.GPCCLEAR.bit.GPIO65 = 1 #define motorDir7Set GpioDataRegs.GPCSET.bit.GPIO66 = 1 //screw #define motorDir7Clear GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1 #define motorDir8Set GpioDataRegs.GPCSET.bit.GPIO67 = 1 #define motorDir8Clear GpioDataRegs.GPCCLEAR.bit.GPIO67 = 1 #define stepSet GpioDataRegs.GPCSET.bit.GPIO69 = 1 // step #define stepClear GpioDataRegs.GPCCLEAR.bit.GPIO69 = 1 #define stepDirSet GpioDataRegs.GPCSET.bit.GPIO70 = 1 //step dir #define stepDirClear GpioDataRegs.GPCCLEAR.bit.GPIO70 = 1 #define I2C_SLAVE_ADDR 0x29 //slave address VL53L0X = 0x29 #define I2C_NUMBYTES 1//4 //#define I2C_EEPROM_HIGH_ADDR 0x00 //#define I2C_EEPROM_LOW_ADDR 0x30 //#define RadToRPM 9.54929658551372L //********************************************************************************************************** //**********************************************interrupts************************************************** //********************************************************************************************************** __interrupt void cpu_timer0_isr(void); //주기 설정용 __interrupt void sciaRxFifoIsr(void); //ARS센서 테이터받기 __interrupt void scibTxFifoIsr(void); //PC로 데이터 전송 __interrupt void scibRxFifoIsr(void); //안드로이드 폰이랑 연결 __interrupt void i2c_int1a_isr(void); //i2c 통신 //********************************************************************************************************** //**********************************************functions*************************************************** //********************************************************************************************************** void IsrSet(void); void SerialTX(void); void Send_Data(void); void SendDataCal(float32 data, float32 data2, float32 data3, float32 data4, float32 data5); void CpuTimer_set(void); void Encoder_cal(void); void AdcResult(void); void Mode_CLRB(void); void Step_control(void); void pass(void); void fail(void); void I2CA_Init(void); float32 LowPassFilter0_5to1_0(float32 signal, float cutoff); Uint16 I2CA_ReadData(struct I2CMSG *msg); Uint16 I2CA_WriteData(struct I2CMSG *msg); //********************************************************************************************************** //**********************************************variables*************************************************** //********************************************************************************************************** //cnts unsigned char cnt_500us = 0; unsigned char cnt_Tx = 0; // gain float32 Screw_kp = 0; float32 YKp = 0; float32 YKd = 0; float32 YKi = 0; float32 adcloop = 0; //flags unsigned char control_flag = 0; unsigned char control_flag_stand_up_B = 0; unsigned char control_flag_stand_up_F = 0; unsigned char control_flag_bal = 0; unsigned char control_flag_Drive = 0; unsigned char SciTxDataSave_flag = 1; unsigned char SciTxReady_flag = 0; unsigned char SensordataReady_flag = 0; unsigned char forward_flag = 0; unsigned char backward_flag = 0; unsigned char left_flag = 0; unsigned char right_flag = 0; unsigned char s_flag = 0; unsigned char q_flag = 0; float32 absEncoder_filter = 2150; float32 absEncoder_x1 = 2150; float32 absEncoder_y1 = 2150; float32 absEncoder = 0; Uint16 absEncoderT1 = 0; Uint16 absEncoderT2 = 0; Uint16 absEncoderT3 = 0; Uint16 absEncoderT4 = 0; int16 Angular_Temp = 0; float32 Angular = 0; float32 Y_Angle = 0; int16 Y_Angle_Temp = 0; int16 R_Angle_Temp = 0; float32 R_Angle = 0; int16 P_Angle_Temp = 0; float32 P_Angle = 0; float32 A_offset = 0; float32 Screw_Err = 0; float32 Screw_Control = 0; float32 Screw_desired = 0; float32 Screw_position = 0; float32 Y_Err = 0; float32 Y_Err_Sum = 0; float32 Y_Err_dt = 0; float32 pre_Y_Err = 0; float32 Y_Control = 0; float32 Y_desired = 0; float32 P_Err = 0; float32 P_Err_Sum = 0; float32 P_Err_dt = 0; float32 pre_P_Err = 0; float32 P_Control = 0; float32 P_desired = 0; float32 F_Total_Control_L = 0; float32 F_Total_Control_R = 0; int32 encoder1_cnt = 0; int32 encoder2_cnt = 0; float32 L_position = 0; //wheel speed filtering float32 P_Err_dt_filter = 0; float32 P_Err_dt_x1 = 0; float32 P_Err_dt_y1 = 0; float32 suction_control = 0; float32 screw_test = 0; float l_m_offset = 400; float r_m_offset = 400; float screw_offset = 1800; unsigned int M_Cleaner_Dir = 0; float32 M_Cleaner_vel = 0; unsigned int rxremainder = 0; unsigned char Rxmode = 0; unsigned char Sdata[42] = {0}; int SensorData[12] = {0}; unsigned char Txbuff[13] = {0}; Uint16 adc_data[8] = {0}; int LT_Adc_data[8] = {0}; float32 Stand = 0; float32 f_dummy = 0; float32 f_dummy_1 = 0; float32 Angle_gain_flag = 0; unsigned char rxData = 0; float32 Define_mode = 10; int count_step = 0; int state_step = 0; float start_flag_step = 0; //Gain/////////////////////////// /* extern Uint16 RamfuncsLoadStart; extern Uint16 RamfuncsLoadEnd; extern Uint16 RamfuncsRunStart; extern Uint16 RamfuncsLoadSize; */ struct I2CMSG I2cMsgOut1={I2C_MSGSTAT_SEND_WITHSTOP, I2C_SLAVE_ADDR} /* I2C_NUMBYTES, I2C_EEPROM_HIGH_ADDR, I2C_EEPROM_LOW_ADDR, 0x12, // Msg Byte 1 0x34, // Msg Byte 2 0x56, // Msg Byte 3 0x78, // Msg Byte 4 0x9A, // Msg Byte 5 0xBC, // Msg Byte 6 0xDE, // Msg Byte 7 0xF0, // Msg Byte 8 0x11, // Msg Byte 9 0x10, // Msg Byte 10 0x11, // Msg Byte 11 0x12, // Msg Byte 12 0x13, // Msg Byte 13 0x12}*/; // Msg Byte 14 struct I2CMSG I2cMsgIn1={ I2C_MSGSTAT_SEND_NOSTOP, I2C_SLAVE_ADDR} /*I2C_NUMBYTES, I2C_EEPROM_HIGH_ADDR, I2C_EEPROM_LOW_ADDR}*/; struct I2CMSG *CurrentMsgPtr; // Used in interrupts Uint16 PassCount; Uint16 FailCount; //Gain////////////////////////////////////////////////////////////////////////////////////////////////////// Uint16 a1 = 0; void main(void) { Uint16 Error; Uint16 i; CurrentMsgPtr = &I2cMsgOut1; // Step 1. Initialize System Control: // PLL, WatchDog, enable Peripheral Clocks // This example function is found in the DSP2833x_SysCtrl.c file. InitSysCtrl(); // Step 2. Initialize GPIO: // This example function is found in the DSP2833x_Gpio.c file and // illustrates how to set the GPIO to it's default state. // 테스트 LED GPIO58 설정 InitGpio(); // SCI GPIO InitSciGpio(); // EPWM GPIO InitEPwmGpio(); // EQEP GPIO InitEQepGpio(); // ECAP GPIO InitECapGpio(); // I2CA GPIO InitI2CGpio(); // Step 3. Clear all interrupts and initialize PIE vector table: // Disable CPU interrupts DINT; // Initialize PIE control registers to their default state. // The default state is all PIE interrupts disabled and flags // are cleared. // This function is found in the DSP2833x_PieCtrl.c file. InitPieCtrl(); // Disable CPU interrupts and clear all CPU interrupt flags: IER = 0x0000; IFR = 0x0000; // Initialize the PIE vector table with pointers to the shell Interrupt // Service Routines (ISR). // This will populate the entire table, even if the interrupt // is not used in this example. This is useful for debug purposes. // The shell ISR routines are found in DSP2833x_DefaultIsr.c. // This function is found in DSP2833x_PieVect.c. InitPieVectTable(); // InitFlash(); // ADC // InitAdc(); // CPU Timer & Timer ISR setting CpuTimer_set(); // SCI InitSci(); // EPWM InitEPwm(); // EQEP InitEQep(); // ECAP InitECap(); //I2C I2CA_Init(); //memcpy(&RamfuncsRunStart, &RamfuncsLoadStart, (Uint32)&RamfuncsLoadSize); // Clear incoming message buffer for (i = 0; i < I2C_MAX_BUFFER_SIZE; i++) { I2cMsgIn1.MsgBuffer[i] = 0x0000; I2cMsgOut1.MsgBuffer[i] = 0x0000; } PassCount = 0; FailCount = 0; // Delay for ARS sensor DELAY_US(500000); // Interrupt service routine IsrSet(); motorDir6Set; // dir pin Set //앞 왼쪽 motorDir5Clear; motorDir1Set; // 오른쪽 motorDir2Clear; motorDir7Set; // Screw motorDir8Clear; stepDirSet; //step dir for(;;) { if(control_flag==1) { Step_control(); Encoder_cal(); Mode_CLRB(); //AdcResult(); //f_dummy++; //----------------------------------------- Heading control if(Angle_gain_flag == 1) { YKp = 1000; YKd = 5; YKi = 0; Screw_kp = 200; } Y_Err = Y_desired - Y_Angle; Y_Err_Sum += Y_Err*Control_period; Y_Err_dt = (P_Err - pre_P_Err)/Control_period; Y_Control=YKp*Y_Err+YKd*Y_Err_dt+YKi*Y_Err_Sum; pre_Y_Err = Y_Err; F_Total_Control_R = -Y_Control + M_Cleaner_vel ; F_Total_Control_L = Y_Control + M_Cleaner_vel ; if(F_Total_Control_R < 0) { F_Total_Control_R = - F_Total_Control_R; motorDir2Set; // 오른쪽 motorDir1Clear; } else { motorDir1Set; // 오른쪽 motorDir2Clear; } if(F_Total_Control_L < 0) { F_Total_Control_L = - F_Total_Control_L; motorDir5Set; // 오른쪽 motorDir6Clear; } else { motorDir6Set; // 오른쪽 motorDir5Clear; } if(Screw_Control < 0) { Screw_Control = - Screw_Control; motorDir7Set; // 오른쪽 motorDir8Clear; } else { motorDir8Set; // 오른쪽 motorDir7Clear; } if(F_Total_Control_R > 7499) F_Total_Control_R = 7499; if(F_Total_Control_L > 7499) F_Total_Control_L = 7499; if(Screw_Control > 7499) Screw_Control = 7499; if(suction_control > 7499) suction_control = 7499; // EPwm1Regs.CMPA.half.CMPA =M_Cleaner_vel+test1; //앞 왼쪽 // EPwm2Regs.CMPB = M_Cleaner_vel+test4;//뒤 오른쪽 EPwm2Regs.CMPA.half.CMPA = Screw_Control + screw_offset; //screw motor -일때 아래로 +일때 위로 EPwm1Regs.CMPB = suction_control; //suction EPwm1Regs.CMPA.half.CMPA = F_Total_Control_R + r_m_offset;//오른쪽 EPwm3Regs.CMPA.half.CMPA = F_Total_Control_L + l_m_offset; //왼쪽 //------------------------------------------------------------------------------------------- ////////////////////////////////// // Write data to EEPROM section // ////////////////////////////////// // Check the outgoing message to see if it should be sent. // In this example it is initialized to send with a stop bit. if(I2cMsgOut1.MsgStatus == I2C_MSGSTAT_SEND_WITHSTOP) { Error = I2CA_WriteData(&I2cMsgOut1); // If communication is correctly initiated, set msg status to busy // and update CurrentMsgPtr for the interrupt service routine. // Otherwise, do nothing and try again next loop. Once message is // initiated, the I2C interrupts will handle the rest. Search for // ICINTR1A_ISR in the i2c_eeprom_isr.c file. if (Error == I2C_SUCCESS) { CurrentMsgPtr = &I2cMsgOut1; I2cMsgOut1.MsgStatus = I2C_MSGSTAT_WRITE_BUSY; } } // end of write section /////////////////////////////////// // Read data from EEPROM section // /////////////////////////////////// // Check outgoing message status. Bypass read section if status is // not inactive. /* if (I2cMsgOut1.MsgStatus == I2C_MSGSTAT_INACTIVE) { // Check incoming message status. if(I2cMsgIn1.MsgStatus == I2C_MSGSTAT_SEND_NOSTOP) { // EEPROM address setup portion while(I2CA_ReadData(&I2cMsgIn1) != I2C_SUCCESS) { // Maybe setup an attempt counter to break an infinite while // loop. The EEPROM will send back a NACK while it is performing // a write operation. Even though the write communique is // complete at this point, the EEPROM could still be busy // programming the data. Therefore, multiple attempts are // necessary. } // Update current message pointer and message status CurrentMsgPtr = &I2cMsgIn1; I2cMsgIn1.MsgStatus = I2C_MSGSTAT_SEND_NOSTOP_BUSY; } // Once message has progressed past setting up the internal address // of the EEPROM, send a restart to read the data bytes from the // EEPROM. Complete the communique with a stop bit. MsgStatus is // updated in the interrupt service routine. else if(I2cMsgIn1.MsgStatus == I2C_MSGSTAT_RESTART) { // Read data portion while(I2CA_ReadData(&I2cMsgIn1) != I2C_SUCCESS) { // Maybe setup an attempt counter to break an infinite while // loop. } // Update current message pointer and message status CurrentMsgPtr = &I2cMsgIn1; I2cMsgIn1.MsgStatus = I2C_MSGSTAT_READ_BUSY; } } // end of read section */ //------------------------------------------------------------------------------------------- control_flag = 0; } //f_dummy++; } } // Step control ----------------------------------------------------------------------------- void Step_control(void) { //250~252가 대략 90도 스텝 카운트 //count_step = 0; //state_step = 0; (0 은 좌 90, 1 은 우90) if(start_flag_step){ stepSet; DELAY_US(500); stepClear; DELAY_US(1000); if(state_step == 0) //좌회전 90도 { count_step++; if(state_step == 250){ stepDirClear; start_flag_step=0; count_step = 0; } } if(state_step == 1) // 우회전 90도 { count_step++; if(count_step == 250){ stepDirSet; start_flag_step = 0; count_step = 0; } } //stepDirSet //stepDirClear } } // Mode control ----------------------------------------------------------------------------- void Mode_CLRB(void) { if(Define_mode == 0) { Y_desired = Y_Angle; if(Y_Err < 0.1 && Y_Err > -0.1) { Angle_gain_flag = 1; } } else if(Define_mode == 1) //forward { M_Cleaner_vel += 200; if(M_Cleaner_vel > 4000) { M_Cleaner_vel = 4000; } } else if(Define_mode == 2) //left { start_flag_step = 1; state_step = 0; Y_desired = Y_desired - 90; if(Y_desired <= -180) { Y_desired = 180; } } else if(Define_mode == 3) //right { start_flag_step = 1; state_step = 1; Y_desired = Y_desired + 90; if(Y_desired >= 180) { Y_desired = 180; } } else if(Define_mode == 4) //backward { M_Cleaner_vel -= 200; if(M_Cleaner_vel < -4000) { M_Cleaner_vel = -4000; } } else if(Define_mode == 5) //screw motor -일때 아래로 +일때 위로 { Screw_desired -= 1; } else if(Define_mode == 6) { Screw_desired += 1; } else if(Define_mode == 7) { suction_control = 1300; } else if(Define_mode == 8) { Y_desired = Y_desired; Screw_desired = Screw_desired; M_Cleaner_vel = 0; } } void I2CA_Init(void) { // Initialize I2C-A: I2caRegs.I2CSAR = 0x0029; // Slave address - EEPROM control code #if (CPU_FRQ_150MHZ) // Default - For 150MHz SYSCLKOUT I2caRegs.I2CPSC.all = 14; // Prescaler - need 7-12 Mhz on module clk (150/15 = 10MHz) #endif #if (CPU_FRQ_100MHZ) // For 100 MHz SYSCLKOUT I2caRegs.I2CPSC.all = 9; // Prescaler - need 7-12 Mhz on module clk (100/10 = 10MHz) #endif I2caRegs.I2CCLKL = 10; // NOTE: must be non zero I2caRegs.I2CCLKH = 5; // NOTE: must be non zero I2caRegs.I2CIER.all = 0x24; // Enable SCD & ARDY interrupts //SCD - I2C 버스에 Stop 신호가 감지 되었을때 인터럽트 발생 //ARDY - 미리 짜여진 주소, 데이터, 명령 값이 사용 되었을때 인터럽트 발생 I2caRegs.I2CMDR.all = 0x0020; // Take I2C out of reset // Stop I2C when suspended I2caRegs.I2CFFTX.all = 0x6000; // Enable FIFO mode and TXFIFO //I2CFFEN 송신/수신 FIFO Enable 비트 //TXFFRST 송신 FIFO 계속 동작 FIFO = First in First out I2caRegs.I2CFFRX.all = 0x2040; // Enable RXFIFO, clear RXFFINT, //RXFFRST 수신 FIFO 계속 동작 //RXFFINTCLR (7번)요청 여부를 표시해 주는 상태 플레그 클리어 return; //tbd... } // ISR setup ----------------------------------------------------------------------------- void IsrSet(void) { EALLOW; // This is needed to write to EALLOW protected registers PieVectTable.SCIRXINTA = &sciaRxFifoIsr; PieVectTable.SCITXINTB = &scibTxFifoIsr; // PieVectTable.SCIRXINTC = &scicRxFifoIsr; PieVectTable.SCIRXINTB = &scibRxFifoIsr; PieVectTable.I2CINT1A = &i2c_int1a_isr; // PieVectTable.ADCINT = &adc_isr_int; EDIS; // This is needed to disable write to EALLOW protected registers PieCtrlRegs.PIECTRL.bit.ENPIE = 1; // Enable the PIE block PieCtrlRegs.PIEIER9.bit.INTx1=1; // Enable PIE Group 9, int1, SCIRXINTA PieCtrlRegs.PIEIER9.bit.INTx4=1; // Enable PIE Group 9, int4, SCITXINTB PieCtrlRegs.PIEIER9.bit.INTx3=1; // Enable PIE Group 9, int4, SCIRXINTB // PieCtrlRegs.PIEIER8.bit.INTx5=1; // Enable PIE Group 8, int5, SCIRXINTC // PieCtrlRegs.PIEIER8.bit.INTx6=1; // Enable PIE Group 8, int5, SCITXINTC // PieCtrlRegs.PIEIER1.bit.INTx6=1; // adc PieCtrlRegs.PIEIER8.bit.INTx1 = 1; // I2C interrupt // IER |= M_INT6; // Enable CPU INT6 IER |= M_INT9; // Enable INT9 // IER |= M_INT1; IER |= M_INT8; // SCIRXINTC //I2C EINT; } void CpuTimer_set() { EALLOW; // This is needed to write to EALLOW protected registers PieVectTable.TINT0 = &cpu_timer0_isr; EDIS; InitCpuTimers(); ConfigCpuTimer(&CpuTimer0, 150, 500); //CPU Timer setting - timer0, 150MHz, 500us(주기) CpuTimer0Regs.TCR.all = 0x4000; // Use write-only instruction to set TSS bit = 0 IER |= M_INT1; PieCtrlRegs.PIEIER1.bit.INTx7 = 1; } // Serial Tx set ----------------------------------------------------------------------------- void SerialTX() //ARS sensor setting { static unsigned char Txmode = 0; Txmode++; Txbuff[0] = 0x02; //start Txbuff[1] = 0x0D; //length Txbuff[2] = 0x01; //device ID Txbuff[12] = 0x03; if(Txmode == 1){ // 비트 Txbuff[3] = 0x18; //오브젝트 쓰기 요청 // 8bit signed int Txbuff[4] = 0x18; //Index- SS = 21(데이터 종류 4=각도) SP = 24(주기 1~6000ms) Txbuff[5] = 0x00; Txbuff[6] = 0x00; //Sub-index Txbuff[7] = 0x01; //data Txbuff[8] = 0x00; Txbuff[9] = 0x00; Txbuff[10] = 0x00; Txbuff[11] = 0x00; //checsum (3rd~11th byte) } else if(Txmode == 2){ // 비트 Txbuff[3] = 0x18; //오브젝트 쓰기 요청 // 8bit signed int Txbuff[4] = 0x15; //Index- SS = 21(데이터 종류 6=각도,각속도/5 = 각도,가속도) SP = 24(주기 1~6000ms) Txbuff[5] = 0x00; Txbuff[6] = 0x00; //Sub-index Txbuff[7] = 0x06; //data Txbuff[8] = 0x00; Txbuff[9] = 0x00; Txbuff[10] = 0x00; Txbuff[11] = 0x00; //checsum (3rd~11th byte) Txmode=0; } SciTxReady_flag=1; } // Send data set ----------------------------------------------------------------------------- void Send_Data() //SerialTX()에서 저장한 13byte 데이터 전송 ARS sensor { static char TXCounts = 0 ; static char Send_cnt=0; if(TXCounts==13) { TXCounts=0; SciTxReady_flag=0; Send_cnt++; if(Send_cnt==2) //13byte 씩 2번 전송이 완료되면 더이상 보내지 않음. { SciTxDataSave_flag=0; Send_cnt=0; } } else { SciaRegs.SCITXBUF=Txbuff[TXCounts]; TXCounts++ ; } } /* void SendDataCal(float32 data, float32 data2, float32 data3, float32 data4, float32 data5) //PC와 통신용으로 ASCII 코드로 변환하는 함수 (미사용) { //100의 자리 , 소수점 2번째 자리 수까지 가능 static unsigned char First_shot = 1; Sdata[0] = '['; if(data<0) { Sdata[1]='-'; data*=-1; } else Sdata[1]=data/1000+48; Sdata[2]=((int)data%1000)/100+48; Sdata[3]=((int)data%100)/10+48; Sdata[4]=((int)data%10)/1+48; Sdata[5]='.'; Sdata[6]=(Uint32)(data*10)%10+48; Sdata[7]=(Uint32)(data*100)%10+48; Sdata[8]=','; if(data2<0) { Sdata[9]='-'; data2*=-1; } else Sdata[9]=data2/1000+48; Sdata[10]=((int)data2%1000)/100+48; Sdata[11]=((int)data2%100)/10+48; Sdata[12]=((int)data2%10)/1+48; Sdata[13]='.'; Sdata[14]=(Uint32)(data2*10)%10+48; Sdata[15]=(Uint32)(data2*100)%10+48; Sdata[16]=','; if(data3<0) { Sdata[17]='-'; data3*=-1; } else Sdata[17]=data3/1000+48; Sdata[18]=((int)data3%1000)/100+48; Sdata[19]=((int)data3%100)/10+48; Sdata[20]=((int)data3%10)/1+48; Sdata[21]='.'; Sdata[22]=(Uint32)(data3*10)%10+48; Sdata[23]=(Uint32)(data3*100)%10+48; Sdata[24]=','; if(data4<0) { Sdata[25]='-'; data4*=-1; } else Sdata[25]=data4/1000+48; Sdata[26]=((int)data4%1000)/100+48; Sdata[27]=((int)data4%100)/10+48; Sdata[28]=((int)data4%10)/1+48; Sdata[29]='.'; Sdata[30]=(Uint32)(data4*10)%10+48; Sdata[31]=(Uint32)(data4*100)%10+48; Sdata[32]=','; if(data5<0) { Sdata[33]='-'; data5*=-1; } else Sdata[33]=data5/1000+48; Sdata[34]=((int)data5%1000)/100+48; Sdata[35]=((int)data5%100)/10+48; Sdata[36]=((int)data5%10)/1+48; Sdata[37]='.'; Sdata[38]=(Uint32)(data5*10)%10+48; Sdata[39]=(Uint32)(data5*100)%10+48; Sdata[40]=']'; Sdata[41]=0x0D; if(First_shot == 1){ ScibRegs.SCIFFTX.bit.TXFIFOXRESET=1; First_shot = 0; } else ScibRegs.SCIFFTX.bit.TXFFINTCLR = 1; } */ // Encoder set ----------------------------------------------------------------------------- void Encoder_cal() { encoder1_cnt=EQep1Regs.QPOSCNT; encoder2_cnt=EQep2Regs.QPOSCNT; L_position = (float)encoder1_cnt; Screw_position = (float)encoder2_cnt; //screw encoder //------------------------------------ screw control Screw_Err = Screw_desired - Screw_position; Screw_Control = Screw_kp * Screw_Err + screw_test; /* //---------------------------------------------//Position Control P_Err = P_desired - (L_position+R_position)/2; P_Err_Sum += P_Err*Control_period; P_Err_dt = (P_Err - pre_P_Err)/Control_period; //wheelMotor1Speed filtering - cut-1Hz LPF @ control freq 100Hz LPF P_Err_dt_filter=0.9391*P_Err_dt_y1+0.0305*P_Err_dt+0.0305*P_Err_dt_x1; P_Err_dt_y1= P_Err_dt_filter; P_Err_dt_x1=P_Err_dt; P_Control=PKp*P_Err+PKd*P_Err_dt_filter+PKi*P_Err_Sum; pre_P_Err = P_Err; */ } /* void AdcResult() { absEncoderT1 = (AdcRegs.ADCRESULT0>>4); absEncoderT2 = (AdcRegs.ADCRESULT1>>4); absEncoderT3 = (AdcRegs.ADCRESULT2>>4); absEncoderT4 = (AdcRegs.ADCRESULT3>>4); absEncoder = ((float32)absEncoderT1+(float32)absEncoderT2+(float32)absEncoderT3+(float32)absEncoderT4)/4; absEncoder_filter=0.9391*absEncoder_y1+0.0305*absEncoder+0.0305*absEncoder_x1; absEncoder_y1=absEncoder_filter; absEncoder_x1=absEncoder; } */ Uint16 I2CA_WriteData(struct I2CMSG *msg) //* 구조체 { Uint16 i; // Wait until the STP bit is cleared from any previous master communication. // Clearing of this bit by the module is delayed until after the SCD bit is // set. If this bit is not checked prior to initiating a new message, the // I2C could get confused. if (I2caRegs.I2CMDR.bit.STP == 1) //STP - 마스터 일때만 사용한다. 1을쓰면 Stop 신호를 발생 후 자동으로 클리어 됨 { return I2C_STP_NOT_READY_ERROR; } // Setup slave address I2caRegs.I2CSAR = msg->SlaveAddress; //SAR 마스터 송신 모드로 동작할 때 송신할 슬레이브 주소를 담는 레지스터. 7bit addr 일때 9-7 비트는 0 // Check if bus busy if (I2caRegs.I2CSTR.bit.BB == 1) //BB - Busy bit, Bus 가 busy 상태일때 1로 set 된다 { return I2C_BUS_BUSY_ERROR; } // Setup number of bytes to send // MsgBuffer + Address // I2caRegs.I2CCNT = msg->NumOfBytes;//+2; //CNT - 송신 모드로 작동할 때 얼마나 많은 데이터 바이트를 송신할 것인지 설정. // Setup data to send // I2caRegs.I2CDXR = msg->MemoryHighAddr; //DXR - 최대 8 비트를 저장할수 있는 레지스터 // I2caRegs.I2CDXR = msg->MemoryLowAddr; for (i=0; i<msg->NumOfBytes; i++) { I2caRegs.I2CDXR = *(msg->MsgBuffer+i); } // Send start as master transmitter f_dummy_1++; I2caRegs.I2CMDR.all = 0x6E20; //FREE, STT, STP, MST, TRX, IRS //FREE - Break Point 에서의 동작을 제어한다. 1일때 break point와 상관없이 모든 동작이 그대로 진행된다. return I2C_SUCCESS; } Uint16 I2CA_ReadData(struct I2CMSG *msg) { // Wait until the STP bit is cleared from any previous master communication. // Clearing of this bit by the module is delayed until after the SCD bit is // set. If this bit is not checked prior to initiating a new message, the // I2C could get confused. if (I2caRegs.I2CMDR.bit.STP == 1) { return I2C_STP_NOT_READY_ERROR; } I2caRegs.I2CSAR = msg->SlaveAddress; if(msg->MsgStatus == I2C_MSGSTAT_SEND_NOSTOP) { // Check if bus busy if (I2caRegs.I2CSTR.bit.BB == 1) { return I2C_BUS_BUSY_ERROR; } I2caRegs.I2CCNT = 2; // I2caRegs.I2CDXR = msg->MemoryHighAddr; // I2caRegs.I2CDXR = msg->MemoryLowAddr; I2caRegs.I2CMDR.all = 0x2620; // Send data to setup EEPROM address //STT - 1을쓰면 start 신호를 발생시킨다 후에 자동으로 클리어한다 IRS = 0 일때는 사용 불가, //MST - 1을 쓰면 마스터가 된다 stop 신호를 발생시키고 나면 0으로 바뀌고 슬레이브가 됨, //TRX - 1을 쓰면 쓰기모드가 됨 0은 읽기모드 //IRS - 0을 쓰면 I2C모듈을 reset/disabled 시키고, I2CSTR(상태비트)가 모두 초기화 됨 1은 Enable } else if(msg->MsgStatus == I2C_MSGSTAT_RESTART) { I2caRegs.I2CCNT = msg->NumOfBytes; // Setup how many bytes to expect I2caRegs.I2CMDR.all = 0x2C20; // Send restart as master receiver //STT, STP - 마스터 일때만 사용한다. 1을쓰면 Stop 신호를 발생 후 자동으로 클리어 됨 //MST, IRS } return I2C_SUCCESS; } /////////////////////////////////// /////////// interrupts //////////// /////////////////////////////////// __interrupt void cpu_timer0_isr(void) { cnt_Tx++; cnt_500us++; if(cnt_Tx==20&&SciTxDataSave_flag&&!SciTxReady_flag) { SerialTX(); cnt_Tx=0; } if(SciTxReady_flag==1) Send_Data(); if(cnt_500us==20) { control_flag=1; cnt_500us=0; } SpiaRegs.SPITXBUF = 0; // Acknowledge this interrupt to receive more interrupts from group 1 PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; } __interrupt void sciaRxFifoIsr(void) { //f_dummy_1++; // ahrs-v2 //dummy++; unsigned char temp=0; temp = SciaRegs.SCIRXBUF.all; //rxData=temp; switch(rxremainder){ case 0: if(temp==0x02) rxremainder++; else rxremainder = 0; break; case 1: rxremainder++; break; case 2: if(temp==0x01) rxremainder++; else rxremainder = 0; break; case 3: if(temp==0xF0) rxremainder++; else rxremainder=0; break; case 4: if(temp==0x34) //0x33 = 가속도, 0x34 = 각속도, 0x35 = 각도(오일러) Rxmode=0; else if(temp==0x35) Rxmode=1; rxremainder++; break; case 5: case 6: SensorData[rxremainder-3]=temp; //Acc y Pitch 3 rxremainder++; break; case 7: case 8: SensorData[rxremainder-7]=temp; //Acc x Roll 1 rxremainder++; break; case 9: case 10: SensorData[rxremainder-5]=temp; //Yaw 5 rxremainder++; break; case 11: rxremainder++; break; default : if(rxremainder==12){ if(Rxmode==1) { //R_Angle_Temp=SensorData[1]*256+SensorData[0]; //R_Angle=(float)R_Angle_Temp/100*-1; //P_Angle_Temp=SensorData[3]*256+SensorData[2]; //P_Angle=(float)P_Angle_Temp/100*-1; Y_Angle_Temp=SensorData[5]*256+SensorData[4]; Y_Angle=(float)Y_Angle_Temp/100*-1; SensordataReady_flag=1; } else if(Rxmode==0) { Angular_Temp=SensorData[5]*256+SensorData[4]; Angular=(float)Angular_Temp/10*-1; SensordataReady_flag=1; } rxremainder = 0; } break; } SciaRegs.SCIFFRX.bit.RXFFOVRCLR=1; // Clear Overflow flag SciaRegs.SCIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag PieCtrlRegs.PIEACK.all|=PIEACK_GROUP9; // Issue PIE ack } __interrupt void scibTxFifoIsr(void) { static unsigned char send_cnt = 0; unsigned char i; if(send_cnt == 0){ for(i=0;i<16;i++) ScibRegs.SCITXBUF = Sdata[i]; send_cnt++; ScibRegs.SCIFFTX.bit.TXFFINTCLR = 1; // Clear SCI Interrupt flag } else if(send_cnt == 1){ for(i=16;i<32;i++) ScibRegs.SCITXBUF = Sdata[i]; send_cnt++; ScibRegs.SCIFFTX.bit.TXFFINTCLR = 1; // Clear SCI Interrupt flag } else if(send_cnt == 2){ for(i=32;i<42;i++) ScibRegs.SCITXBUF = Sdata[i]; send_cnt = 0; } PieCtrlRegs.PIEACK.all |= PIEACK_GROUP9; // Issue PIE ACK } /* __interrupt void scicRxFifoIsr(void) { f_dummy++; unsigned char temp3 = 0; //temp3 = ScicRegs.SCIRXBUF.all; temp3 = ScicRegs.SCIRXBUF.bit.RXDT; rxData = temp3; switch(temp3){ case 0: Define_mode = 0; break; case 1: Define_mode = 1; break; case 2: Define_mode = 2; break; case 3: Define_mode = 3; break; case 4: Define_mode = 4; break; case 5: break; case 6: break; case 7: break; default: break; } ScicRegs.SCIFFRX.bit.RXFFOVRCLR=1; ScicRegs.SCIFFRX.bit.RXFFINTCLR=1; PieCtrlRegs.PIEACK.all|=PIEACK_GROUP8; } */ __interrupt void scibRxFifoIsr(void) { unsigned char temp3 = 0; temp3 = ScibRegs.SCIRXBUF.all; rxData = temp3; // F , L , R , B , S; T; D; U; V; switch(temp3){ case 83://Stop Define_mode = 0; break; case 70://Forward Define_mode = 1; break; case 76://Left Define_mode = 2; break; case 82://Right Define_mode = 3; break; case 66://Backward Define_mode = 4; break; case 68://D Suction down Define_mode = 5; break; case 85://U Suction up Define_mode = 6; break; case 86://V Vaccum_motor Define_mode = 7; break; case 84://T Touch Up Define_mode = 8; break; default: break; } ScibRegs.SCIFFRX.bit.RXFFOVRCLR=1; ScibRegs.SCIFFRX.bit.RXFFINTCLR=1; PieCtrlRegs.PIEACK.all|=PIEACK_GROUP9; } __interrupt void i2c_int1a_isr(void) { f_dummy++; Uint16 IntSource, i; // Read interrupt source IntSource = I2caRegs.I2CISRC.all; // Interrupt source = stop condition detected if(IntSource == I2C_SCD_ISRC) { // If completed message was writing data, reset msg to inactive state if (CurrentMsgPtr->MsgStatus == I2C_MSGSTAT_WRITE_BUSY) { CurrentMsgPtr->MsgStatus = I2C_MSGSTAT_INACTIVE; } else { // If a message receives a NACK during the address setup portion of the // EEPROM read, the code further below included in the register access ready // interrupt source code will generate a stop condition. After the stop // condition is received (here), set the message status to try again. // User may want to limit the number of retries before generating an error. if(CurrentMsgPtr->MsgStatus == I2C_MSGSTAT_SEND_NOSTOP_BUSY) { CurrentMsgPtr->MsgStatus = I2C_MSGSTAT_SEND_NOSTOP; } // If completed message was reading EEPROM data, reset msg to inactive state // and read data from FIFO. else if (CurrentMsgPtr->MsgStatus == I2C_MSGSTAT_READ_BUSY) { CurrentMsgPtr->MsgStatus = I2C_MSGSTAT_INACTIVE; for(i=0; i < I2C_NUMBYTES; i++) { CurrentMsgPtr->MsgBuffer[i] = I2caRegs.I2CDRR; } { // Check recieved data for(i=0; i < I2C_NUMBYTES; i++) { if(I2cMsgIn1.MsgBuffer[i] == I2cMsgOut1.MsgBuffer[i]) { PassCount++; } else { FailCount++; } } if(PassCount == I2C_NUMBYTES) { pass(); } else { fail(); } } } } } // end of stop condition detected // Interrupt source = Register Access Ready // This interrupt is used to determine when the EEPROM address setup portion of the // read data communication is complete. Since no stop bit is commanded, this flag // tells us when the message has been sent instead of the SCD flag. If a NACK is // received, clear the NACK bit and command a stop. Otherwise, move on to the read // data portion of the communication. else if(IntSource == I2C_ARDY_ISRC) { if(I2caRegs.I2CSTR.bit.NACK == 1) { I2caRegs.I2CMDR.bit.STP = 1; I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT; } else if(CurrentMsgPtr->MsgStatus == I2C_MSGSTAT_SEND_NOSTOP_BUSY) { CurrentMsgPtr->MsgStatus = I2C_MSGSTAT_RESTART; } } // end of register access ready else { // Generate some error due to invalid interrupt source __asm(" ESTOP0"); } // Enable future I2C (PIE Group 8) interrupts PieCtrlRegs.PIEACK.all = PIEACK_GROUP8; } void pass() { __asm(" ESTOP0"); for(;;); } void fail() { __asm(" ESTOP0"); for(;;); }
void InitI2CGpio()
{
EALLOW;
/* Enable internal pull-up for the selected pins */
// Pull-ups can be enabled or disabled disabled by the user.
// This will enable the pullups for the specified pins.
// Comment out other unwanted lines.
GpioCtrlRegs.GPBPUD.bit.GPIO32 = 0; // Enable pull-up for GPIO32 (SDAA)
GpioCtrlRegs.GPBPUD.bit.GPIO33 = 0; // Enable pull-up for GPIO33 (SCLA)
/* Set qualification for selected pins to asynch only */
// This will select asynch (no qualification) for the selected pins.
// Comment out other unwanted lines.
GpioCtrlRegs.GPBQSEL1.bit.GPIO32 = 3; // Asynch input GPIO32 (SDAA)
GpioCtrlRegs.GPBQSEL1.bit.GPIO33 = 3; // Asynch input GPIO33 (SCLA)
/* Configure SCI pins using GPIO regs*/
// This specifies which of the possible GPIO pins will be I2C functional pins.
// Comment out other unwanted lines.
GpioCtrlRegs.GPBMUX1.bit.GPIO32 = 1; // Configure GPIO32 for SDAA operation
GpioCtrlRegs.GPBMUX1.bit.GPIO33 = 1; // Configure GPIO33 for SCLA operation
EDIS;
}
