void main()
{
static TICK t = 0;
CAN_MESSAGE outCm;
// Inits
mainInit();
#ifdef USE_CAN
canInit();
#endif
tickInit();
// Read ID and NID if exist.
if (EERead(NODE_ID_EE)==NODE_HAS_ID)
{
MY_ID=(((WORD)EERead(NODE_ID_EE + 1))<<8)+EERead(NODE_ID_EE + 2);
MY_NID=EERead(NODE_ID_EE + 3);
}
// Send user program startup heatbeat
outCm.funct = FUNCT_BOOTLOADER;
outCm.funcc = FUNCC_BOOT_HEARTBEAT;
outCm.nid = MY_NID;
outCm.sid = MY_ID;
outCm.data_length = 1;
outCm.data[BOOT_DATA_HEARTBEAT_INDEX] = HEARTBEAT_USER_STARTUP;
while(!canSendMessage(outCm,PRIO_HIGH));
while(1)
{
static TICK t = 0;
static TICK heartbeat = 0;
if ((tickGet()-heartbeat)>TICK_SECOND*5)
{
// Send alive heartbeat
outCm.funct = FUNCT_BOOTLOADER;
outCm.funcc = FUNCC_BOOT_HEARTBEAT;
outCm.nid = MY_NID;
outCm.sid = MY_ID;
outCm.data_length = 1;
outCm.data[BOOT_DATA_HEARTBEAT_INDEX] = HEARTBEAT_ALIVE;
while(!canSendMessage(outCm,PRIO_HIGH));
heartbeat = tickGet();
}
if ((tickGet()-t)>TICK_SECOND)
{
LED0_IO=~LED0_IO;
t = tickGet();
}
}
}
/*
* .KB_C_FN_DEFINITION_START
* void WriteEEPROM(unsigned ee_off, char *data_addr, unsigned size)
* This global function writes data to the eeprom at ee_off using data
* from data_addr for size bytes. Assume the TWI has been initialized.
* This function does not utilize the page write mode as the write time is
* much greater than the time required to access the device for byte-write
* functionality. This allows the function to be much simpler.
* .KB_C_FN_DEFINITION_END
*/
void
EEWrite(unsigned ee_off, const char *data_addr, unsigned size)
{
const AT91PS_TWI twiPtr = AT91C_BASE_TWI;
unsigned status;
char test_data;
while (size--) {
// Set the TWI Master Mode Register
twiPtr->TWI_MMR = (iicaddr(ee_off) << 16) |
AT91C_TWI_IADRSZ_1_BYTE;
twiPtr->TWI_IADR = ee_off++;
status = twiPtr->TWI_SR;
// Load one data byte
twiPtr->TWI_THR = *(data_addr++);
twiPtr->TWI_CR = AT91C_TWI_START;
while (!(twiPtr->TWI_SR & AT91C_TWI_TXRDY))
continue;
twiPtr->TWI_CR = AT91C_TWI_STOP;
status = twiPtr->TWI_SR;
while (!(twiPtr->TWI_SR & AT91C_TWI_TXCOMP))
continue;
// wait for write operation to complete, it is done once
// we can read it back...
EERead(ee_off, &test_data, 1);
}
}
void RadioInit(void) {
TI_CC_SPISetup(); // Initialize SPI port del PIC
TI_CC_PowerupResetCCxxxx(); // Reset CCxxxx
IoWait(200);
WriteRFSettings(); // Write RF settings to config reg
TI_CC_SPIWriteBurstReg(TI_CCxxx0_PATABLE, paTable, paTableLen);//Write PATABLE
ModeRecepcio();
// Capturo l'adreça local i l'adreça del control remot des de l'eeprom.
EEGetShortMAC(LocalAddress);
#ifdef _DEPURANT_RADIO
RemoteAddress[0] = AdressaRemotaFalsa[0];
RemoteAddress[1] = AdressaRemotaFalsa[1];
RemoteAddress[2] = AdressaRemotaFalsa[2];
#else
EERead(EE_MAC_REMOT,RemoteAddress, RF_LEN_ADDRESS);
// Versió 0.2
if ((RemoteAddress[0] == 0xFF) && (RemoteAddress[1] == 0xff)) ModeLite = 1;
else ModeLite = 0;
#endif
}
//
//----------------------------------------PÚBLIQUES----------------
//
void MtInit(void){
// Programo els ports d'stop
TRISDbits.TRISD8 = 1; // STOP0
TRISDbits.TRISD9 = 1; // STOP1
TRISDbits.TRISD10 = 1; // STOP2
TRISDbits.TRISD11 = 1; // STOP3
TRISBbits.TRISB15 = 1; // STOP4
TRISGbits.TRISG9 = 1; // STOP5
TRISBbits.TRISB14 = 1; // STOP6
TRISCbits.TRISC14 = 1; // STOP7
// Programo els ports de control dels motors
TRISEbits.TRISE5 = 0;
TRISEbits.TRISE6 = 0;
TRISEbits.TRISE7 = 0;
TRISBbits.TRISB9 = 0;
TRISBbits.TRISB10 = 0; // Dir0
TRISBbits.TRISB11 = 0; // Dir1
TRISBbits.TRISB12 = 0; // Dir2
TRISBbits.TRISB13 = 0; // Dir3
// Els que poden ser analògics cal reconfigurar-los, però ja ho ha fet l'init d'IO
// Paro els motors
ENABLE0 = 0;
ENABLE1 = 0;
ENABLE2 = 0;
ENABLE3 = 0;
// Recullo la calibració de l'eprom
// en CalEsquerra, CalDreta
// --------------------------------------PENDENT--------------------
// Programo els PWM dels motors
// Usaré el Timer2 com a base de temps comú
OC1CON = 0; // De moment, parat
OC1R = 50;
OC1RS = 50;
OC1CON = 0x0006; // Acció per a OC1 (PWM0)
OC2CON = 0; // De moment, parat
OC2R = 50;
OC2RS = 50;
OC2CON = 0x0006; // Acció per a OC2 (PWM1)
//-----------------------------Servo en PW2 i PW3
// Programo el timer 3 per a una frequencia de 2us. Comptant que el preescaler rep 4M (8Mhz / 2)
T3CONbits.TCKPS0 = 1; // Preescaler de timer3 a 1:8
T3CONbits.TCKPS1 = 0; // Preescaler de timer3 a 1:8
T3CONbits.TON = 1; // Activo timer
// Inicialitza la sortida de PWM2 (OC3) per ajustar la durada del pols entre 1 i 2 ms
OC3CON = 0; // De moment, parat
OC3R = 500;
OC3RS = 500;
OC3CON = 0x0006 | 8 ; // Que depengui del timer 3, en mode PMW
// El port RD2 de sortida (OC3)
TRISDbits.TRISD2 = 0;
// Amb 10us de tic a timer 3, tenim 1ms si posem 500 a OC3RS i de 2ms si posem 1000 a OC3RS
// el periode total el fixem amb PR2 a 25 ms (12500)
PR3 = 12500;
// Si es vol, es pot habilitat un altre servo a OC4 compartint el timer 3
// El port RD3 de sortida (OC4)
TRISDbits.TRISD3 = 0;
OC4CON = 0; // De moment, parat
OC4R = 500;
OC4RS = 500;
OC4CON = 0x0006 | 8 ; // Que depengui del timer 3, en mode PMW
// Els dos servos a la posicio neutre (50)
OC3RS = 500+50*5;
OC4RS = 500+50*5;
PosServo2 = PosServo3 = 50;
//-----------------------------End Servo en PW2 i PW3
// El timer 2 genera una freqüencia de 39680Hz per a un valor de 100 en PR2
// si treballem a 8MHz i fixem el preescaler a 64
PR2 = 100;
// Preescaler a 1
T2CONbits.TCKPS0 = 0; // Preescaler de timer2 a 1
T2CONbits.TCKPS1 = 0; // Preescaler de timer2 a 1
T2CONbits.TON = 1; // Activo timer
HiHaOrdre = 0;
T = TiGetTimer();
// Agafo les dades de calibració
if (EEVerge()) {
// Els valors per defecte estan a dalt de tot del fitxer
} else {
EERead(EE_CALIBRA_ESQUERRA, (unsigned char*)&CalEsquerra, 1);
EERead(EE_CALIBRA_DRETA, (unsigned char*)&CalDreta, 1);
EERead(EE_CALIBRA_TRACCIO, (unsigned char*)&CalTraccio, 1);
CorreccioTraccio = (50 - CalTraccio); // De +50 a -50
CorreccioTraccio = 1 + CorreccioTraccio / 100.0; // de 0.5 a 1.5
// LS_USB_printf("Correcio Traccio %f\n", CorreccioTraccio);
}
V = W = Rx = Ry = Dy = 0;
Dx = 1;
NivellStops = 1;
}
/*
* .KB_C_FN_DEFINITION_START
* void ParseCommand(char *)
* This private function executes matching functions.
* .KB_C_FN_DEFINITION_END
*/
static void
ParseCommand(char *buffer)
{
int argc, i;
if ((argc = BreakCommand(buffer)) < 1)
return;
switch (StringToCommand(argv[0])) {
case COMMAND_DUMP:
// display boot commands
DumpBootCommands();
break;
case COMMAND_EXEC:
{
// "e <address>"
// execute at address
void (*execAddr)(unsigned, unsigned);
if (argc > 1) {
/* in future, include machtypes (MACH_KB9200 = 612) */
execAddr = (void (*)(unsigned, unsigned))
p_ASCIIToHex(argv[1]);
(*execAddr)(0, 612);
}
break;
}
case COMMAND_TFTP:
{
// "tftp <local_dest_addr filename>"
// tftp download
unsigned address = 0;
if (argc > 2)
address = p_ASCIIToHex(argv[1]);
TFTP_Download(address, argv[2]);
break;
}
case COMMAND_SERVER_IP:
// "server_ip <server IP 192 200 1 20>"
// set download server address
if (argc > 4)
SetServerIPAddress(BuildIP());
break;
case COMMAND_LOCAL_IP:
// "local_ip <local IP 192 200 1 21>
// set ip of this module
if (argc > 4)
SetLocalIPAddress(BuildIP());
break;
case COMMAND_MAC:
{
// "m <mac address 12 34 56 78 9a bc>
// set mac address using 6 byte values
unsigned char mac[6];
if (argc > 6) {
for (i = 0; i < 6; i++)
mac[i] = p_ASCIIToHex(argv[i + 1]);
EMAC_SetMACAddress(mac);
}
break;
}
case COMMAND_LOAD_SPI_KERNEL:
// "k <address>"
if (argc > 1)
LoadKernelFromSpi((char *)p_ASCIIToHex(argv[1]));
break;
case COMMAND_XMODEM:
// "x <address>"
// download X-modem record at address
if (argc > 1)
xmodem_rx((char *)p_ASCIIToHex(argv[1]));
break;
case COMMAND_RESET:
printf("Reset\n");
reset();
while (1) continue;
break;
case COMMAND_REPLACE_KERNEL_VIA_XMODEM:
printf("Updating KERNEL image\n");
UpdateFlash(KERNEL_OFFSET);
break;
case COMMAND_REPLACE_FLASH_VIA_XMODEM:
printf("Updating FLASH image\n");
UpdateFlash(FLASH_OFFSET);
break;
case COMMAND_REPLACE_ID_EEPROM:
{
char buf[25];
printf("Testing Config EEPROM\n");
EEWrite(0, "This is a test", 15);
EERead(0, buf, 15);
printf("Found '%s'\n", buf);
break;
}
default:
break;
}
printf("\n");
}
