void check_sonar(char num, char rising)
{
if(rising) { // Début du pulse, on enregistre le temps.
rangers[num].pulse_start = time;
}
else { // Fin du pulse.
char new_state;
char state_changed;
rangers[num].value = time - rangers[num].pulse_start;
// marche aussi si le timer a débordé car non signé
// TODO échelle
new_state = (rangers[num].value < rangers[num].threshold);
state_changed = (rangers[num].state != new_state);
if(rangers[num].unmuted || state_changed)
{
rangers[num].state = new_state;
while(!CANSendMessage(352 | new_state << 4 | state_changed << 3 | num, (BYTE*)&(rangers[num].value), 2,
CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME )) {
}
led = led ^ 1;
}
}
}
void check_button(char num, char pin)
{
if(pin == switches[num]) // Évènement sur la pin.
{
unsigned int id = 272 | (!pin << 3) | num;
switches[num] = !pin; // Résistances pull-up => niveaux inversés.
if(num == 2)
id |= 1920; // diffusion
while(!CANSendMessage(id, NULL, 0,
CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME )) {
}
led = led ^ 1;
}
}
void sendCellBalancingStatus()
{
if(sendCellBalancingStatus_send==1 && CANIsTxReady()){
sendCellBalancingStatus_send=0;
#define CAN_lenghth3 6
CAN_Data[0]=BatteryCells[0].PWM;
CAN_Data[1]=BatteryCells[1].PWM;
CAN_Data[2]=BatteryCells[2].PWM;
CAN_Data[3]=BatteryCells[3].PWM;
CAN_Data[4]=BatteryCells[4].PWM;
CAN_Data[5]=BatteryCells[5].PWM;
// CAN adresses
//todo read from eeprom
CAN_Identifier = (((unsigned long)0x18) << 24) | (((unsigned long)0xff) << 16) | (((unsigned long)bms_id) << 8) | (unsigned long)0x12;
//send message
CANSendMessage(CAN_Identifier, &CAN_Data[0], CAN_lenghth3, CAN_TX_PRIORITY_0 & CAN_TX_XTD_FRAME & CAN_TX_NO_RTR_FRAME);
}
}
void sendBmsStatus()
{
if(sendBmsStatus_send==1 && CANIsTxReady()){
sendBmsStatus_send=0;
#define CAN_lenghth1 7
CAN_Data[0]=BmsStatus0;
CAN_Data[1]=BmsStatus1;
CAN_Data[2]=Temperature[0];
CAN_Data[3]=Temperature[1];
CAN_Data[4]=Temperature[2];
CAN_Data[5]=Temperature[3];
CAN_Data[6]=bms_id;
// CAN adresses
//todo read from eeprom
CAN_Identifier = (((unsigned long)0x18) << 24) | (((unsigned long)0xff) << 16) | (((unsigned long)bms_id) << 8) | (unsigned long)0x10;
//send message
CANSendMessage(CAN_Identifier, &CAN_Data[0], CAN_lenghth1, CAN_TX_PRIORITY_0 & CAN_TX_XTD_FRAME & CAN_TX_NO_RTR_FRAME);
}
}
void sendCellMaxVoltage()
{
if(sendCellMaxVoltage_send==1 && CANIsTxReady()){
sendCellMaxVoltage_send=0;
// CAN data
#define CAN_lenghth5 6
CAN_Data[0]=BatteryCells[0].MaxVoltage;
CAN_Data[1]=BatteryCells[1].MaxVoltage;
CAN_Data[2]=BatteryCells[2].MaxVoltage;
CAN_Data[3]=BatteryCells[3].MaxVoltage;
CAN_Data[4]=BatteryCells[4].MaxVoltage;
CAN_Data[5]=BatteryCells[5].MaxVoltage;
// CAN adresses
//todo read from eeprom
CAN_Identifier = (((unsigned long)0x18) << 24) | (((unsigned long)0xff) << 16) | (((unsigned long)bms_id) << 8) | (unsigned long)0x13;
//send message
CANSendMessage(CAN_Identifier, &CAN_Data[0], CAN_lenghth5, CAN_TX_PRIORITY_0 & CAN_TX_XTD_FRAME & CAN_TX_NO_RTR_FRAME);
}
}
void low_isr(void) {
if (INTCONbits.TMR0IE && INTCONbits.TMR0IF) {
/* L'interruption TMR0 gère la fréquence de calcul des asservs. Elle soit être assez
rapide pour assurer de ma réactivité, mais pas trop longue pour collecter assez de ticks
entre deux calculs. La fréquence est fixée à environ 30Hz. */
/* Calcul vitesse */
gVitesse = gTicks - gTicksp;
dVitesse = dTicks - dTicksp;
gTicksp = gTicks;
dTicksp = dTicks;
#ifdef GROS
gVitesse = gVitesse;
dVitesse = dVitesse;
#endif
/* Calcul erreur vitesse */
dErreur = dConsigne - dVitesse;
gErreur = gConsigne - gVitesse;
dIErreur += dErreur;
gIErreur += gErreur;
#ifdef GROS
if( (fabs(gIErreur) > 5000) || (fabs(dIErreur) > 5000) ) //reset si overflow
{
GsetDC(0);
DsetDC(0);
CANSendMessage(1056, (BYTE*)&ticksElie, 0,
CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME);
DelayMS(100);
_asm
reset
_endasm
}
void main(void)
{
WDTCON = 0x00;
//TRISA = 0xC0;
TRISB = 0xFF;
TRISC = 0xFF;
UartTxCnt = 0;
UartRxCnt = 0;
CanRxBuff = 0;
UartTxBuff = 0;
// Initialize TIMER0
INTCON2bits.TMR0IP = 0; //Timer0 INT-LOW
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_128);
// 24Mhz/4/128 := 46875 -> 65536 - 46875 = 18661
WriteTimer0(49911); // 1Sec interval at 8Mhz
/* EEAddres.Bytes[0] = Read_b_eep(0);
EEAddres.Bytes[1] = Read_b_eep(1);
EEAddres.Bytes[2] = Read_b_eep(2);
EEAddres.Bytes[3] = Read_b_eep(3);
EEAddres.SE_ID = 0x20123456;
*/
// Initialize CAN module with no message filtering
// 8MHz Fosc 250Kb/s
// 8MHz -> 2MHz @ 250Khz = 8Tq (1+2+3+2)
CANInitialize(1 ,0x02 ,2 ,3 ,2 , CAN_CONFIG_VALID_XTD_MSG); //256kb at 8Mhz crystal
/*CANSetOperationMode(CAN_OP_MODE_CONFIG);
CANSetFilter(CAN_FILTER_B1_F1, Can_bootF.SE_ID, CAN_CONFIG_XTD_MSG);
CANSetFilter(CAN_FILTER_B1_F2, Can_nodeF.SE_ID, CAN_CONFIG_XTD_MSG);
CANSetMask(CAN_MASK_B1, Can_Mask1.SE_ID, CAN_CONFIG_XTD_MSG);
CANSetFilter(CAN_FILTER_B2_F1, Can_addrLO.SE_ID, CAN_CONFIG_XTD_MSG);
CANSetMask(CAN_MASK_B2, Can_Mask2.SE_ID, CAN_CONFIG_XTD_MSG);
*/
CANSetOperationMode(CAN_OP_MODE_NORMAL);
//ADC configuration
//reference is connected to A0
//voltage output connected to A1
LATA = 0x00;
PORTA=0;
TRISA=0xFF; //A port as input
ADCON1 = 0b00111101;//VSS,VDD ref. AN0 analog only
//ADCON2 = 0b00001001;//ADCON2 setup: Left justified, Tacq=2Tad, Tad=2*Tosc (or Fosc/2)
ADCON2 = 0b10101011;
ADCON0bits.ADON = 0x01;//Enable A/D module
INTCONbits.GIE = 1; //enable global interrupts
Tcombo int2byte;
UartRx_Msg.Data[4] = 1;//CanRx_Msg.Data[0];
UartRx_Msg.Data[5] = 2;//CanRx_Msg.Data[0];
UartRx_Msg.Data[6] = 3;//CanRx_Msg.Data[7];
UartRx_Msg.Data[7] = 4;//CanRx_Msg.Data[7];
while(1)
{
ClrWdt();
/********************************/
//cobtinuasly reading of current*/
/********************************/
//ADC chanel A0 - voltage
ADCON0bits.CHS0 = 0;//clear ADCON0 to select channel 0 (AN0)
ADCON0bits.CHS1 = 0;//clear ADCON0 to select channel 0 (AN0)
ADCON0bits.CHS2 = 0;//clear ADCON0 to select channel 0 (AN0)
ADCON0bits.CHS3 = 0;//clear ADCON0 to select channel 0 (AN0)
Delay100TCYx (2);
ADCON0bits.GO = 1;
while (ADCON0bits.GO); //wait for conversion
current_ad_value=ADRES;
ClrWdt();
Delay100TCYx (2);
//ADC chanel A1 - rederence
ADCON0bits.CHS0 = 1; //select channel 1 (AN1)
Delay100TCYx (2);
ADCON0bits.GO = 1;
while (ADCON0bits.GO); //wait for conversion
reference_ad_value= ADRES;
//delta ADC = reference votlatge - real voltage
//we filter it thru buffer
count++;
if(count>=100) count=0;
delta_ad[count] = reference_ad_value-current_ad_value;
if (UTicTac >= 1) {
UTicTac = 0;
//current conversion
//I=1.6V*200/1024*0.625 * delta_ad ==0.52083333333333333333333333333333 *delta_ad
//reference votlatge - real voltage
current=0;
for(char i=0;i<100;i++){
current+=delta_ad[i];
}
//current *= 0.0052083; //above formula /100
current *= 0.00463541637; //above formula with correction factor of 0.89
int2byte.Int = (int)current;
UartRx_Msg.Data[0] = int2byte.Char[1];
UartRx_Msg.Data[1] = int2byte.Char[0];
//capcity
capacity += (current/3600);
int2byte.Int=(int)capacity;
UartRx_Msg.Data[2] = int2byte.Char[1];
UartRx_Msg.Data[3] = int2byte.Char[0];
// CAN adresses
UartRx_Msg.Address.SE_ID = 0x010000F2;
CANSendMessage(UartRx_Msg.Address.SE_ID, &UartRx_Msg.Data[0], 8, CAN_TX_PRIORITY_0 & CAN_TX_XTD_FRAME & CAN_TX_NO_RTR_FRAME);
ClrWdt();
}
if (CANIsRxReady()) // Check for CAN message
{
CANReceiveMessage(&CanRx_Msg.Address.SE_ID, &CanRx_Msg.Data[0], &CanRx_Msg.Length.Len, &RecFlags);
// if ( RecFlags & CAN_RX_OVERFLOW )
// {
// // Rx overflow occurred; handle it
// }
if ( RecFlags & CAN_RX_INVALID_MSG )
{ // Invalid message received; handle it
}
else
{
/* if ( RecFlags & CAN_RX_RTR_FRAME )
{ // RTR frame received
//UartRx_Msg.Data[6]++;
CanRxBuff++;
if (CanRxBuff >= BuffNO)
CanRxBuff = 0;
}
else
{ // Regular frame received.
//UartRx_Msg.Data[7]++;
CanRxBuff++;
if (CanRxBuff >= BuffNO)
CanRxBuff = 0;
}*/
if(CanRx_Msg.Address.SE_ID==0x020000F2) capacity=(char)CanRx_Msg.Data[0];
/*UartRx_Msg.Data[4] = CanRx_Msg.Data[0];
UartRx_Msg.Data[5] = CanRx_Msg.Data[1];
UartRx_Msg.Data[6] = CanRx_Msg.Data[6];
UartRx_Msg.Data[7] = CanRx_Msg.Data[7];*/
}
// if ( RecFlags & CAN_RX_XTD_FRAME )
// {
// // Extended Identifier received; handle it
// }
// else
// {
// // Standard Identifier received.
// }
// // Extract receiver filter match, if it is to be used
// RxFilterMatch = RecFlags & CAN_RX_FILTER_BITS;
}
// Process received message
/* if ((BusyUSART() == 0) && (UartSync == 1))
{
if (UartTxCnt > 0) // Preveri èe posiljamo CAN telegram na UART
{
WriteUSART(CanRx_Msg[UartTxBuff].Array[UartTxCnt]); // Pošlji znak
UartTxCnt++; // poveèaj stevec za naslednji znak
if ((UartTxCnt) >= (J1939_MSG_LENGTH + J1939_DATA_LENGTH)) // Preveri èe je zadni znak
{
UartTxCnt = 0; // Postavi na prvi znak
UartTxBuff++; // Premakni na novi Buffer
if (UartTxBuff >= BuffNO) // Preveri da nismo cez mejo
UartTxBuff =0;
}
}
else
{
if (CanRxBuff != UartTxBuff)
{
WriteUSART(CanRx_Msg[UartTxBuff].Array[UartTxCnt]); // Pošlji ga na UART
UartTxCnt++; // Poveèaj števec na naslednji znak
}
}
}
if (UartRxCnt >= (J1939_MSG_LENGTH + J1939_DATA_LENGTH)) // Ali imamo vse znake.
{
if (CANIsTxReady()) // Preveri ali ja CAN prost
{
// void CANSendMessage(unsigned long id, BYTE *Data, BYTE DataLen enum CAN_TX_MSG_FLAGS MsgFlags);
CANSendMessage(UartRx_Msg.Address.SE_ID, &UartRx_Msg.Data[0], 8, CAN_TX_PRIORITY_0 & CAN_TX_XTD_FRAME & CAN_TX_NO_RTR_FRAME);
UartRxCnt = 0; // Pripravi za sprejem novega telegrama iz UART-a
LATAbits.LATA5 = !LATAbits.LATA5;
}
}
if DataRdyUSART() // Preveri ali v UART èaka nov znak
{
UTicTac = 0;
tp_char = ReadUSART(); // Preber znak v polje
if (UartSync)
{
UartRx_Msg.Array[UartRxCnt] = tp_char;
UartRxCnt++; // in poveèaj stevec prebranih znakov iz UART-a
}
else
{
if (tp_char == '#')
UartSync = 1;
}
}
if (UTicTac > 5) // Èe je števec èez mejo, telegram ni veljeven
{
UartRxCnt = 0; // Postavi na prvi znak v telegramu
UTicTac = 0;
}*/
} // Do this forever
}
void low_isr(void)
{
// Réception CAN.
if(PIE3bits.RXB0IE && PIR3bits.RXB0IF)
{
unsigned long id;
char num;
short cmd;
BYTE data[8];
BYTE len;
enum CAN_RX_MSG_FLAGS flags;
while(CANIsRxReady()) {
CANReceiveMessage(&id, data, &len, &flags);
}
PIR3bits.RXB0IF = 0;
led = led ^ 1;
num = id & 0x07;
cmd = id & 0xFFF8;
if(cmd == 320) { // rangerReq
id = 352 | (rangers[num].value < rangers[num].threshold) << 4 | num;
while(!CANSendMessage(id, (BYTE*)rangers[num].value, 2,
CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME )) {
}
}
else if(cmd == 328) { // rangerThres
rangers[num].threshold = ((unsigned int*) data)[0];
}
else if(cmd == 336) { // rangerMute
rangers[num].unmuted = 0;
}
else if(cmd == 344) { // rangerUnmute
rangers[num].unmuted = 1;
}
else {
led = led ^ 1; // On annule la commutation précédente de la LED.
}
}
// Génération des pulses sonars et polling des bumpers.
if(INTCONbits.TMR0IE && INTCONbits.TMR0IF) // Vingt fois par secondes, non stop.
{
INTCONbits.TMR0IF = 0;
check_button(0, PORTCbits.RC2);
check_button(1, PORTCbits.RC3);
check_button(2, PORTCbits.RC4);
check_button(3, PORTCbits.RC5);
// On alterne les triggers pulse, et pas besoin
// d'attendre plus car ils restent allumés la moitié du temps (50ms).
if(PORTCbits.RC7) {
CloseRB1INT();
OpenRB0INT(PORTB_CHANGE_INT_ON & RISING_EDGE_INT & PORTB_PULLUPS_OFF);
PORTCbits.RC6 = 1;
PORTCbits.RC7 = 0; // Fin du pulse => déclenchement.
}
else {
CloseRB0INT();
OpenRB1INT(PORTB_CHANGE_INT_ON & RISING_EDGE_INT & PORTB_PULLUPS_OFF);
PORTCbits.RC6 = 0;
PORTCbits.RC7 = 1; // Fin du pulse => déclenchement.
}
// Début de l'attente des echos.
// Lecture de l'ADC pour les sharps et passage à la mesure suivante.
//count_sharp_value = (count_sharp_value + 1) %3;
//count_sharp_value_tab = count_sharp_value + 3*(cur_sharp - 2); // On se place au bon endroit dans le tableau sharp_value[].
count_sharp_value_tab = count_sharp_value + 3*(cur_sharp - 2);
count_sharp_value_change = (count_sharp_value_change + 1) %3;
if (count_sharp_value_change == 2)
count_sharp_value = (count_sharp_value + 1)%3;
count_sharp_value_change = (count_sharp_value_change + 1) %3;
sharp_value[count_sharp_value_tab] = LectureAnalogique();
rangers[cur_sharp].value = (sharp_value[3*(cur_sharp - 2)] + sharp_value[1 + 3*(cur_sharp - 2)] + sharp_value[2 + 3*(cur_sharp - 2)])/3;
//rangers[cur_sharp].value = LectureAnalogique();
if(rangers[cur_sharp].unmuted) {
while(!CANSendMessage(352 | cur_sharp, (BYTE*)&(rangers[cur_sharp].value), 2,
CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME )) {
}
led = led ^ 1;
}
if(++cur_sharp > 4)
cur_sharp = 2;
// Lancement de la conversion suivante.
ADCON0 = 0b00000001 + ((cur_sharp - 2) << 2); // Selectionne le bon AN en lecture analogique
//ADCON2 peut être configuré si on le souhaite
ADCON0bits.GO_DONE=1;
//ADCON0bits.CHS = cur_sharp; // Plus simple que SetChanADC(). //TODO
//ConvertADC(); //TODO
}
}
