void Particles2D::_notification(int p_what) {
if (p_what == NOTIFICATION_DRAW) {
RID texture_rid;
if (texture.is_valid())
texture_rid = texture->get_rid();
RID normal_rid;
if (normal_map.is_valid())
normal_rid = texture->get_rid();
VS::get_singleton()->canvas_item_add_particles(get_canvas_item(), particles, texture_rid, normal_rid, h_frames, v_frames);
#ifdef TOOLS_ENABLED
if (Engine::get_singleton()->is_editor_hint() && (this == get_tree()->get_edited_scene_root() || get_tree()->get_edited_scene_root()->is_a_parent_of(this))) {
draw_rect(visibility_rect, Color(0, 0.7, 0.9, 0.4), false);
}
#endif
}
if (p_what == NOTIFICATION_PAUSED || p_what == NOTIFICATION_UNPAUSED) {
if (can_process()) {
VS::get_singleton()->particles_set_speed_scale(particles, speed_scale);
} else {
VS::get_singleton()->particles_set_speed_scale(particles, 0);
}
}
if (p_what == NOTIFICATION_TRANSFORM_CHANGED) {
_update_particle_emission_transform();
}
}
void AudioStreamPlayer::_notification(int p_what) {
if (p_what == NOTIFICATION_ENTER_TREE) {
AudioServer::get_singleton()->add_callback(_mix_audios, this);
if (autoplay && !Engine::get_singleton()->is_editor_hint()) {
play();
}
}
if (p_what == NOTIFICATION_INTERNAL_PROCESS) {
if (!active || (setseek < 0 && !stream_playback->is_playing())) {
active = false;
set_process_internal(false);
emit_signal("finished");
}
}
if (p_what == NOTIFICATION_EXIT_TREE) {
AudioServer::get_singleton()->remove_callback(_mix_audios, this);
}
if (p_what == NOTIFICATION_PAUSED) {
if (!can_process()) {
// Node can't process so we start fading out to silence
set_stream_paused(true);
}
}
if (p_what == NOTIFICATION_UNPAUSED) {
set_stream_paused(false);
}
}
int
main(void) {
/*Enable interrupts*/
INTCON2bits.GIE = 1; //disabled by the bootloader, so we must absolutely enable this!!!
asm("CLRWDT"); //clear watchdog timer
for (torque = 0; torque < 65533; torque++) {
Nop();
}
InitBoard(&ADCBuff, &uartBuffer, &spiBuffer, EventChecker);
if (can_motor_init()) {
while (1);
}
CB_Init(&uartBuffer, uartBuf, 32);
CB_Init(&spiBuffer, (uint8_t *) spiBuf, 128);
LED1 = 1;
LED2 = 0;
LED3 = 1;
LED4 = 1;
#ifdef POSITION
/* This is used for testing */
// SetPosition(incPos);
#endif
#ifdef VELOCITY
SetVelocity(0);
#endif
while (1) {
if (events & EVENT_UPDATE_SPEED) {
PMSM_Update_Position();
events &= ~EVENT_UPDATE_SPEED;
}
if (events & EVENT_CAN) {
can_process();
// LED2 = !LED2;
if (txreq_bitarray & 0b00000001 && !C1TR01CONbits.TXREQ0) {
C1TR01CONbits.TXREQ0 = 1;
txreq_bitarray = txreq_bitarray & 0b11111110;
}
if (txreq_bitarray & 0b00000010 && !C1TR01CONbits.TXREQ1) {
C1TR01CONbits.TXREQ1 = 1;
txreq_bitarray = txreq_bitarray & 0b11111101;
}
if (txreq_bitarray & 0b00000100 && !C1TR23CONbits.TXREQ2) {
C1TR23CONbits.TXREQ2 = 1;
txreq_bitarray = txreq_bitarray & 0b11111011;
}
if (txreq_bitarray & 0b00001000 && !C1TR23CONbits.TXREQ3) {
C1TR23CONbits.TXREQ3 = 1;
txreq_bitarray = txreq_bitarray & 0b11110111;
}
if (txreq_bitarray & 0b00010000 && !C1TR45CONbits.TXREQ4) {
C1TR45CONbits.TXREQ4 = 1;
txreq_bitarray = txreq_bitarray & 0b11101111;
}
if (txreq_bitarray & 0b00100000 && !C1TR45CONbits.TXREQ5) {
C1TR45CONbits.TXREQ5 = 1;
txreq_bitarray = txreq_bitarray & 0b11011111;
}
if (txreq_bitarray & 0b01000000 && !C1TR67CONbits.TXREQ6) {
C1TR67CONbits.TXREQ6 = 1;
txreq_bitarray = txreq_bitarray & 0b10111111;
}
SetPosition(((float) CO(position_control_Commanded_Position)) * 109. / 1000.);
CO(state_Current_Position) = CO(position_control_Commanded_Position);
can_time_dispatch();
events &= ~EVENT_CAN;
}
if (events & EVENT_UART_DATA_READY) {
events &= ~EVENT_UART_DATA_READY;
}
if (events & EVENT_SPI_RX) {
static uint8_t message[32];
uint16_t size;
uint8_t out[56];
message[0] = 0xFF;
message[1] = 0xFF;
message[2] = 0xFF;
message[3] = 0xFF;
message[4] = 0xFF;
message[5] = 0xFF;
message[6] = 0xFF;
message[7] = 0xFF;
CB_ReadByte(&spiBuffer, &message[0]);
CB_ReadByte(&spiBuffer, &message[1]);
CB_ReadByte(&spiBuffer, &message[2]);
CB_ReadByte(&spiBuffer, &message[3]);
CB_ReadByte(&spiBuffer, &message[4]);
CB_ReadByte(&spiBuffer, &message[5]);
CB_ReadByte(&spiBuffer, &message[6]);
CB_ReadByte(&spiBuffer, &message[7]);
CB_Init(&spiBuffer, &spiBuf, 64);
events &= ~EVENT_SPI_RX;
}
if (events & EVENT_REPORT_FAULT) {
events &= ~EVENT_REPORT_FAULT;
}
}
}
int
main(int argc, char** argv)
{
uint32_t led_colors = 0;
uint8_t at_parm_test[10];
unsigned once;
uint32_t i = 0;
uint8_t canPrescaler = 0;
uint8_t* uart_tx_packet = 0;
uint8_t* uart_rx_packet;
uint32_t old_loadcell_data;
uint16_t timeStep = 1;
clock_init();
pin_init();
led_init();
timers_init();
state_init();
uart_init();
// Set up UART2 for 115200 baud. There's no round() on the dsPICs, so we implement our own.
double brg = (double) 140000000 / 2.0 / 16.0 / 115200.0 - 1.0;
if (brg - floor(brg) >= 0.5) {
brg = ceil(brg);
}
else {
brg = floor(brg);
}
// Uart2Init (brg); // Init UART 2 as 115200 baud/s
loadcell_init();
loadcell_start();
led_rgb_off();
led_rgb_set(50, 0, 100);
can_state.init_return = RET_UNKNOWN;
if (can_init()) {
while (1);
}
timer_state.prev_systime = 0;
timer_state.systime = 0;
#ifdef CONF71
// Start Reading the int pin on IMU
mpuData.startData = 0;
if (IMU_Init(400000, 70000000) == 0) {
// imu_state.init_return = RET_OK;
mpuData.startData = 1;
}
else {
//imu_state.init_return = RET_ERROR;
}
#endif
for (;;) {
if (timer_state.systime != timer_state.prev_systime) {
timer_state.prev_systime = timer_state.systime;
//everything in here will be executed once every ms
//make sure that everything in here takes less than 1ms
//useful for checking state consistency, synchronization, watchdog...
if(timer_state.systime % 1000 == 1) {
LED_4 = 1;
}
led_update();
if (timer_state.systime % 10 == 1) {
IMU_GetQuaternion(quaterion);
QuaternionToYawPitchRoll(quaterion, ypr);
}
if (timer_state.systime % 5 == 1) {
IMU_normalizeData(&mpuData, &magData, &imuData);
// Run AHRS algorithm
IMU_UpdateAHRS (&imuData);
// Run IMU algorithm (does not use MAG data)
// IMU_UpdateIMU(&imuData);
//copy state to CAN dictionary
IMU_CopyOutput(&imuData, &mpuData, &magData);
}
/**
* CANFestival Loop
*/
if (can_state.init_return == RET_OK) {
can_process();
/**
* Sets CANFestival shared variables
* specific to Sensor Board
*/
can_push_state();
}
/**
* Blinking LED Loop
*/
if (timer_state.systime % 25 == 0) {
LED_1 = !LED_1;
}
}
else {
//untimed processes in main loop:
//executed as fast as possible
//these processes should NOT block the main loop
// LED_4 = mpuData.accelX > 0;
// LED_3 = mpuData.accelY > 0;
// LED_1 = mpuData.accelZ > 0;
// IMU_GetData();
IMU_CopyI2CData(&mpuData, &magData);
if (!T1CONbits.TON) {
RGB_RED = 0;
RGB_GREEN = RGB_BLUE = 1;
while (1);
}
if(can_flag){
TimeDispatch();
can_flag = 0;
}
uart_rx_packet = uart_rx_cur_packet();
if (uart_rx_packet != 0) {
uart_rx_packet_consumed();
}
/**
* Handles CAN transmission buffers
*/
if ((txreq_bitarray & 0b00000001) && !C1TR01CONbits.TXREQ0) {
C1TR01CONbits.TXREQ0 = 1;
txreq_bitarray = txreq_bitarray & 0b11111110;
}
if ((txreq_bitarray & 0b00000010) && !C1TR01CONbits.TXREQ1) {
C1TR01CONbits.TXREQ1 = 1;
txreq_bitarray = txreq_bitarray & 0b11111101;
}
if ((txreq_bitarray & 0b00000100) && !C1TR23CONbits.TXREQ2) {
C1TR23CONbits.TXREQ2 = 1;
txreq_bitarray = txreq_bitarray & 0b11111011;
}
if ((txreq_bitarray & 0b00001000) && !C1TR23CONbits.TXREQ3) {
C1TR23CONbits.TXREQ3 = 1;
txreq_bitarray = txreq_bitarray & 0b11110111;
}
if ((txreq_bitarray & 0b00010000) && !C1TR45CONbits.TXREQ4) {
C1TR45CONbits.TXREQ4 = 1;
txreq_bitarray = txreq_bitarray & 0b11101111;
}
if ((txreq_bitarray & 0b00100000) && !C1TR45CONbits.TXREQ5) {
C1TR45CONbits.TXREQ5 = 1;
txreq_bitarray = txreq_bitarray & 0b11011111;
}
if ((txreq_bitarray & 0b01000000) && !C1TR67CONbits.TXREQ6) {
C1TR67CONbits.TXREQ6 = 1;
txreq_bitarray = txreq_bitarray & 0b10111111;
}
}
}
return (EXIT_SUCCESS);
}
