/**
* @brief HAL initialization.
*/
void halInit(void) {
hal_lld_init();
#if CH_HAL_USE_PAL
palInit(&pal_default_config);
#endif
#if CH_HAL_USE_ADC
adcInit();
#endif
#if CH_HAL_USE_CAN
canInit();
#endif
#if CH_HAL_USE_MAC
macInit();
#endif
#if CH_HAL_USE_PWM
pwmInit();
#endif
#if CH_HAL_USE_SERIAL
sdInit();
#endif
#if CH_HAL_USE_SPI
spiInit();
#endif
#if CH_HAL_USE_MMC_SPI
mmcInit();
#endif
}
/**
* @brief HAL initialization.
* @details This function invokes the low level initialization code then
* initializes all the drivers enabled in the HAL. Finally the
* board-specific initialization is performed by invoking
* @p boardInit() (usually defined in @p board.c).
*
* @init
*/
void halInit(void) {
hal_lld_init();
#if HAL_USE_TM || defined(__DOXYGEN__)
tmInit();
#endif
#if HAL_USE_PAL || defined(__DOXYGEN__)
palInit(&pal_default_config);
#endif
#if HAL_USE_ADC || defined(__DOXYGEN__)
adcInit();
#endif
#if HAL_USE_CAN || defined(__DOXYGEN__)
canInit();
#endif
#if HAL_USE_EXT || defined(__DOXYGEN__)
extInit();
#endif
#if HAL_USE_GPT || defined(__DOXYGEN__)
gptInit();
#endif
#if HAL_USE_I2C || defined(__DOXYGEN__)
i2cInit();
#endif
#if HAL_USE_ICU || defined(__DOXYGEN__)
icuInit();
#endif
#if HAL_USE_MAC || defined(__DOXYGEN__)
macInit();
#endif
#if HAL_USE_PWM || defined(__DOXYGEN__)
pwmInit();
#endif
#if HAL_USE_SERIAL || defined(__DOXYGEN__)
sdInit();
#endif
#if HAL_USE_SDC || defined(__DOXYGEN__)
//KL All in Kl_sdc sdcInit();
#endif
#if HAL_USE_SPI || defined(__DOXYGEN__)
spiInit();
#endif
#if HAL_USE_UART || defined(__DOXYGEN__)
uartInit();
#endif
#if HAL_USE_USB || defined(__DOXYGEN__)
usbInit();
#endif
#if HAL_USE_MMC_SPI || defined(__DOXYGEN__)
mmcInit();
#endif
#if HAL_USE_SERIAL_USB || defined(__DOXYGEN__)
sduInit();
#endif
#if HAL_USE_RTC || defined(__DOXYGEN__)
rtcInit();
#endif
}
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();
}
}
}
void bspInit(void) {
lowLevelInit();
ledsInit();
buttonsInit();
adcInit();
accelerometerInit();
potentiometerInit();
lcdInit();
canInit();
}
void J1939_init(unsigned char startAddress, unsigned char *name)
{
unsigned char i=0;
TXstart=TXend=RXstart=RXend=0;
TXsize=RXsize=0;
canInit(CAN_SPEED); // 250 Kbit/s
ComandedAddress = startAddress;
J1939_Address = startAddress;
for (i;i<8;i++) Name.name[i] = name[i];
// Set up mask 0 to receive broadcast messages. Set up mask 1 to
// receive messages sent to the global address (or eventually us).
RXM0SIDH = 0x07;
RXM0SIDL = 0x88; //0x88;
RXM0EIDH = 0x00;
RXM0EIDL = 0x00;
RXM1SIDH = 0x00;
RXM1SIDL = 0x08;
RXM1EIDH = 0xFF;
RXM1EIDL = 0x00;
// Set up filter 0 to accept only broadcast messages (PF = 240-255).
// Set up filter 2 and 3 to accept only the global address. Once we
// get an address for the CA, we'll change filter 3 to accept that
// address.
RXF0SIDH = 0x07;
RXF0SIDL = 0x88; //0x88
RXF2SIDL = 0x08;
RXF2EIDH = J1939_GLOBAL_ADDRESS;
RXF3SIDL = 0x08;
RXF3EIDH = J1939_GLOBAL_ADDRESS;
canMode(0);
J1939_Flags.CannotClaimAddress = 0;
J1939_Flags.WaitingForAddressClaimContention = 0;
J1939_Flags.ReceivedMessagesDropped = 0;
J1939_Flags.AddressRequest = 0;
J1939_ClaimAddress(J1939_CLAIM_ADDRESS_TX);
while (J1939_Flags.WaitingForAddressClaimContention)
J1939_poll(5);
return;
}
Thread* CanInitLocal(void){
canInit();
canStart(&CAND1, &cancfg);
can_tp = chThdCreateFromHeap(
&ThdHeap,
sizeof(CanTxThreadWA),
LINK_THREADS_PRIO - 2,
CanTxThread,
NULL);
if (can_tp == NULL)
chDbgPanic("Can not allocate memory");
return can_tp;
}
static void motion_control_thread(void *arg)
{
initTimer(); //初始化CANopen定时器
if(1 == ROBOT_BODY) //创建本体控制进程
{
CO_D.CO_CAN1 = &ObjDict_CAN1_Data;
CO_D.CO_CAN1->canHandle = CAN1;
//创建主履带消息队列
xQ_DRIVE_COMM = xQueueCreate( 5, sizeof(NET_ROBOT_CONTROL_DRIVER_MESSAGE_STRUCT));
//创建摆臂消息队列
xQ_FLIP_COMM = xQueueCreate( 5, sizeof(NET_ROBOT_CONTROL_FLIPPER_MESSAGE_STRUCT));
//创建本体控制进程
xTaskCreate(body_control_thread, "body_control", BODY_CONTROL_THREAD_STACK, CO_D.CO_CAN1,BODY_CONTROL_THREAD_PRIO, &xH_Body);
if(NULL == xH_Body)
{
printf("!!body control thread create error \r\n");
}
//else
//printf("body control thread created\r\n");
}
if(1 == ROBOT_WITH_ARM) //创建机械手控制进程
{
canInit(CAN2,CAN_BAUD_1M); //初始化CAN2
////////
CO_D.CO_CAN2 = &ObjDict_CAN2_Data; //给结构体指针地址，随对象字典名字不同而不同
//创建操作臂消息队列
xQ_ARM_COMM = xQueueCreate( 5, sizeof(NET_ROBOT_CONTROL_ARM_MESSAGE_STRUCT) );
//创建机械手消息队列
xQ_HAND_COMM = xQueueCreate( 5, sizeof(NET_ROBOT_CONTROL_HAND_MESSAGE_STRUCT) );
//创建机械手控制进程
xTaskCreate(arm_control_thread, "arm_control", ARM_CONTROL_THREAD_STACK, CO_D.CO_CAN2,ARM_CONTROL_THREAD_PRIO, &xH_Arm);
if(NULL == xH_Arm)
{
printf("!!arm control thread create error \r\n"); //创建进程出错
}
else
printf("arm control thread created\r\n");
}
while(1)
{
vTaskDelay(1000);
//vTaskSuspend( NULL ); //挂起自己
}
}
void main(void) {
/* USER CODE BEGIN (3) */
/* Initialize sci for communication over USB */
sciInit();
sciSend(scilinREG,19,(unsigned char *)"CAN code start...\r\n");
printf("Code start...\r\n");
/* enable irq interrupt in Cortex R4 */
_enable_interrupt_();
/** - writing a random data in RAM - to transmit */
//dumpSomeData();
/** - configuring CAN1 MB1,Msg ID-1 to transmit and CAN2 MB1 to receive */
canInit();
/** - enabling error interrupts */
canEnableErrorNotification(canREG1);
canEnableErrorNotification(canREG2);
//while(1){}; /* wait forever after tx-rx complete. */
//ADC stuff
adcInit();
adcData_t adc_data[16];
adcData_t *ptr = &adc_data[0];
int j;
uint8 tx_data[D_COUNT][8];
uint8 *tx_ptr = &tx_data[0][0];
while(1) {
for(j=0U;j<15000;j++){
adc_convert_all_channels(ptr);
int i;
for(i=0;i<sizeof(adc_data)/sizeof(adc_data[0]);i++) {
can_transmit_recieve(canREG1, canMESSAGE_BOX1, &adc_data[i]);
}
}
//for(i=0;i<sizeof(tx_data)/sizeof(tx_data[0]);i++) {
// can_transmit_recieve(canREG1, canMESSAGE_BOX1, &tx_data[i]);
//}
}
//exit(0);
/* USER CODE END */
}
int main(void)
{
sys_init(); // Initialize system
canInit(CAN_BAUDRATE); // Initialize the CANopen bus
initTimer(); // Start timer for the CANopen stack
nodeID = read_bcd(); // Read node ID first
setNodeId (&ObjDict_Data, nodeID);
setState(&ObjDict_Data, Initialisation); // Init the state
for(;;) // forever loop
{
if (sys_timer) // Cycle timer, invoke action on every time slice
{
reset_sys_timer(); // Reset timer
digital_input[0] = get_inputs();
digital_input_handler(&ObjDict_Data, digital_input, sizeof(digital_input));
digital_output_handler(&ObjDict_Data, digital_output, sizeof(digital_output));
set_outputs(digital_output[0]);
// Check if CAN address has been changed
if(!( nodeID == read_bcd()))
{
nodeID = read_bcd(); // Save the new CAN adress
setState(&ObjDict_Data, Stopped); // Stop the node, to change the node ID
setNodeId(&ObjDict_Data, nodeID); // Now the CAN adress is changed
setState(&ObjDict_Data, Pre_operational); // Set to Pre_operational, master must boot it again
}
}
// a message was received pass it to the CANstack
if (canReceive(&m)) // a message reveived
canDispatch(&ObjDict_Data, &m); // process it
else
{
// Enter sleep mode
#ifdef WD_SLEEP // Watchdog and Sleep
wdt_reset();
sleep_enable();
sleep_cpu();
#endif // Watchdog and Sleep
}
}
}
return_value_t can_init()
{
can_state.init_return = RET_OK;
if(!canInit(1000))
{
//fail
can_state.init_return = RET_ERROR;
return can_state.init_return;
}
LED_1;
initTimer();
//initialize CanFestival
//reset callback
Sensor_Board_Data.NMT_Slave_Node_Reset_Callback = can_reset;
#ifdef CONF71
setNodeId(&Sensor_Board_Data, 0x71);
#else
setNodeId(&Sensor_Board_Data, 0x01);
#endif
can_state.is_master = 1;
setState(&Sensor_Board_Data, Initialisation); // Init the state
setState(&Sensor_Board_Data, Operational);
#ifdef CONF71
can_enable_slave_heartbeat(0x73,200);
can_enable_slave_heartbeat(0x72,200);
can_enable_heartbeat(200);
#else
can_enable_slave_heartbeat(0x04,200);
can_enable_slave_heartbeat(0x03,200);
can_enable_slave_heartbeat(0x02,200);
can_enable_heartbeat(200);
#endif
PDOInit(&Sensor_Board_Data);
return can_state.init_return;
}
/**
* @brief HAL initialization.
* @details This function invokes the low level initialization code then
* initializes all the drivers enabled in the HAL. Finally the
* board-specific initialization is performed by invoking
* @p boardInit() (usually defined in @p board.c).
*
* @init
*/
void halInit(void) {
/* Initializes the OS Abstraction Layer.*/
osalInit();
/* Platform low level initializations.*/
hal_lld_init();
#if (HAL_USE_PAL == TRUE) || defined(__DOXYGEN__)
palInit(&pal_default_config);
#endif
#if (HAL_USE_ADC == TRUE) || defined(__DOXYGEN__)
adcInit();
#endif
#if (HAL_USE_CAN == TRUE) || defined(__DOXYGEN__)
canInit();
#endif
#if (HAL_USE_DAC == TRUE) || defined(__DOXYGEN__)
dacInit();
#endif
#if (HAL_USE_EXT == TRUE) || defined(__DOXYGEN__)
extInit();
#endif
#if (HAL_USE_GPT == TRUE) || defined(__DOXYGEN__)
gptInit();
#endif
#if (HAL_USE_I2C == TRUE) || defined(__DOXYGEN__)
i2cInit();
#endif
#if (HAL_USE_I2S == TRUE) || defined(__DOXYGEN__)
i2sInit();
#endif
#if (HAL_USE_ICU == TRUE) || defined(__DOXYGEN__)
icuInit();
#endif
#if (HAL_USE_MAC == TRUE) || defined(__DOXYGEN__)
macInit();
#endif
#if (HAL_USE_PWM == TRUE) || defined(__DOXYGEN__)
pwmInit();
#endif
#if (HAL_USE_SERIAL == TRUE) || defined(__DOXYGEN__)
sdInit();
#endif
#if (HAL_USE_SDC == TRUE) || defined(__DOXYGEN__)
sdcInit();
#endif
#if (HAL_USE_SPI == TRUE) || defined(__DOXYGEN__)
spiInit();
#endif
#if (HAL_USE_UART == TRUE) || defined(__DOXYGEN__)
uartInit();
#endif
#if (HAL_USE_USB == TRUE) || defined(__DOXYGEN__)
usbInit();
#endif
#if (HAL_USE_MMC_SPI == TRUE) || defined(__DOXYGEN__)
mmcInit();
#endif
#if (HAL_USE_SERIAL_USB == TRUE) || defined(__DOXYGEN__)
sduInit();
#endif
#if (HAL_USE_RTC == TRUE) || defined(__DOXYGEN__)
rtcInit();
#endif
#if (HAL_USE_WDG == TRUE) || defined(__DOXYGEN__)
wdgInit();
#endif
/* Community driver overlay initialization.*/
#if defined(HAL_USE_COMMUNITY) || defined(__DOXYGEN__)
#if (HAL_USE_COMMUNITY == TRUE) || defined(__DOXYGEN__)
halCommunityInit();
#endif
#endif
/* Board specific initialization.*/
boardInit();
/*
* The ST driver is a special case, it is only initialized if the OSAL is
* configured to require it.
*/
#if OSAL_ST_MODE != OSAL_ST_MODE_NONE
stInit();
#endif
}
void main(void)
{
static TICK t = 0;
// Initialize any application specific hardware.
InitializeBoard();
canInit();
// Initialize Stack and application related NV variables.
InitAppConfig();
StackInit();
#if defined(STACK_USE_HTTP_SERVER)
HTTPInit();
#endif
#if defined(STACK_USE_SGP_SERVER)
SGPInit();
#endif
// Once all items are initialized, go into infinite loop and let
// stack items execute their tasks.
// If application needs to perform its own task, it should be
// done at the end of while loop.
// Note that this is a "co-operative mult-tasking" mechanism
// where every task performs its tasks (whether all in one shot
// or part of it) and returns so that other tasks can do their
// job.
// If a task needs very long time to do its job, it must broken
// down into smaller pieces so that other tasks can have CPU time.
while(1)
{
// Blink SYSTEM LED every second.
if ( tickGet()-t >= TICK_1S/2 )
{
t = tickGet();
LED0_IO ^= 1;
}
// This task performs normal stack task including checking
// for incoming packet, type of packet and calling
// appropriate stack entity to process it.
StackTask();
#if defined(STACK_USE_HTTP_SERVER)
// This is a TCP application. It listens to TCP port 80
// with one or more sockets and responds to remote requests.
HTTPServer();
#endif
#if defined(STACK_USE_SGP_SERVER)
// This is a TCP application. It listens to TCP port 6666
// with one or more sockets and responds to remote requests.
SGPServer();
#endif
#if defined(STACK_USE_TIMESYNC)
timeSync();
#endif
// In future, as new TCP/IP applications are written, it
// will be added here as new tasks.
// Add your application speicifc tasks here.
ProcessIO();
}
}
void main(void) {
rccInit();
statusLed = digitalInit(GPIO_STATUS_LED_PORT, GPIO_STATUS_LED_PIN);
errorLed = digitalInit(GPIO_ERROR_LED_PORT, GPIO_ERROR_LED_PIN);
#ifdef ESC_DEBUG
tp = digitalInit(GPIO_TP_PORT, GPIO_TP_PIN);
digitalLo(tp);
#endif
timerInit();
configInit();
adcInit();
fetInit();
serialInit();
canInit();
runInit();
cliInit();
owInit();
runDisarm(REASON_STARTUP);
inputMode = ESC_INPUT_PWM;
fetSetDutyCycle(0);
fetBeep(200, 100);
fetBeep(300, 100);
fetBeep(400, 100);
fetBeep(500, 100);
fetBeep(400, 100);
fetBeep(300, 100);
fetBeep(200, 100);
pwmInit();
digitalHi(statusLed);
digitalHi(errorLed);
// self calibrating idle timer loop
{
uint32_t lastRunCount;
uint32_t thisCycles, lastCycles;
volatile uint32_t cycles;
volatile uint32_t *DWT_CYCCNT = (uint32_t *)0xE0001004;
volatile uint32_t *DWT_CONTROL = (uint32_t *)0xE0001000;
volatile uint32_t *SCB_DEMCR = (uint32_t *)0xE000EDFC;
*SCB_DEMCR = *SCB_DEMCR | 0x01000000;
*DWT_CONTROL = *DWT_CONTROL | 1; // enable the counter
minCycles = 0xffff;
while (1) {
idleCounter++;
if (runCount != lastRunCount && !(runCount % (RUN_FREQ / 1000))) {
if (commandMode == CLI_MODE)
cliCheck();
else
binaryCheck();
lastRunCount = runCount;
}
thisCycles = *DWT_CYCCNT;
cycles = thisCycles - lastCycles;
lastCycles = thisCycles;
// record shortest number of instructions for loop
totalCycles += cycles;
if (cycles < minCycles)
minCycles = cycles;
}
}
}
