// ------------------------------------------------------------------------- void mcp2515_bit_modify(uint8_t adress, uint8_t mask, uint8_t data) { RESET(MCP2515_CS); spi_putc(SPI_BIT_MODIFY); spi_putc(adress); spi_putc(mask); spi_putc(data); SET(MCP2515_CS); }
uint8_t sja1000_read(uint8_t address) { uint8_t data; // set address SET(SJA1000_ALE); PORT(SJA1000_DATA) = address; _NOP(); RESET(SJA1000_ALE); DDR(SJA1000_DATA) = 0; // read data RESET(SJA1000_RD); _NOP(); data = PIN(SJA1000_DATA); SET(SJA1000_RD); DDR(SJA1000_DATA) = 0xff; return data; }
Processor::Processor() { REGISTERS = gcnew array<BYTE>(7); memory = gcnew array<BYTE>(65536); if(!REGISTERS || !memory) { throw gcnew SystemException("Cannot allocate memory on heap."); } RESET(); }
/* check the checksum */ void checksum(void) { g2x(); if(cksum!=0) // if checksum does not add to zero, bad check, reset RESET(); #if VERBOSE == 2 putch('\r'); // echo each hex record on a new line putch('\n'); // echo each hex record on a new line #endif }
void UT_TMceStartingAnswererStreams::SetupL() { CMccControllerStub::OpenL(); iMcc = CMccControllerStub::Stub(); iServer = new (ELeave) CMceServerStub(); iManager = CMceMediaManager::NewL( *iServer ); iSdpDocument = CSdpDocument::DecodeL( KMceMMTestSdpAnswerAudioIMS5PrecsSendrecv ); iSession = MceMediaManagerTestHelper::CreateAudioOutSessionL(); SETUP(); RESET(); }
void terminal_operation(ringbuf_t *tx, char *op0, char *op1, char *op2, char *op3) { if (!strcmp(op0, "help") || !strcmp(op0, "?")) { ringbuf_put_str(tx, "\tprint_time(t)\n" "\tadjust_time(at) [hand:y,M,d,h,m,s] [value(decimal)]\n" "\tpwm [color:R,G,B,W] [value(%):0~100]\n" "\tpwm clear\n" "\tset time_sync_mode(sync)\n" "\tset ADC_mode(adc)\n" "\tRESET();\n" ); } terminal_time(tx, op0, op1, op2); if (!strcmp(op0, "RESET();")) { RESET(); } if (!strcmp(op0, "bootload")) { asm("goto 0x001C"); } if (!strcmp(op0, "set")) { if (!strcmp(op1, "sync")) { light_mode = TIME_SYNC_mode; } if (!strcmp(op1, "adc") || !strcmp(op1, "ADC")) { light_mode = ADC_mode; } } if (!strcmp(op0, "pwm") || !strcmp(op0, "PWM")) { if (!strcmp(op1, "r") || !strcmp(op1, "R")) { light_mode = COMMAND_mode; PWM_set(0, atoi(op2) * PR_VALUE / 100); } if (!strcmp(op1, "g") || !strcmp(op1, "G")) { light_mode = COMMAND_mode; PWM_set(1, atoi(op2) * PR_VALUE / 100); } if (!strcmp(op1, "b") || !strcmp(op1, "B")) { light_mode = COMMAND_mode; PWM_set(2, atoi(op2) * PR_VALUE / 100); } if (!strcmp(op1, "w") || !strcmp(op1, "W")) { light_mode = COMMAND_mode; PWM_set(3, atoi(op2) * PR_VALUE / 100); } if (!strcmp(op1, "clear")) { light_mode = COMMAND_mode; PWM_set(0, 0); PWM_set(1, 0); PWM_set(2, 0); PWM_set(3, 0); } } }
void mcp2515_init(void) { // Aktivieren der Pins fuer das SPI Interface PORT_SPI &= ~((1 << PIN_NUM(P_SCK)) | (1 << PIN_NUM(P_MOSI))); DDR_SPI |= (1 << PIN_NUM(P_SCK)) | (1 << PIN_NUM(P_MOSI)); SET(MCP2515_CS); SET_OUTPUT(MCP2515_CS); // Aktivieren des SPI Master Interfaces SPCR = (1 << SPE) | (1 << MSTR) | R_SPCR; SPSR = R_SPSR; _delay_us(1); // MCP2515 per Software Reset zuruecksetzten, // danach ist er automatisch im Konfigurations Modus RESET(MCP2515_CS); spi_putc(SPI_RESET); SET(MCP2515_CS); // ein bisschen warten bis der MCP2515 sich neu gestartet hat _delay_ms(0.1); // Filter usw. setzen RESET(MCP2515_CS); spi_putc(SPI_WRITE); spi_putc(RXF0SIDH); for (uint8_t i = 0; i < sizeof(mcp2515_register_map); i++) { spi_putc(pgm_read_byte(&mcp2515_register_map[i])); } SET(MCP2515_CS); // nur Standard IDs, Message Rollover nach Puffer 1 mcp2515_write_register(RXB0CTRL, (0 << RXM1) | (1 << RXM0) | (1 << BUKT)); mcp2515_write_register(RXB1CTRL, (0 << RXM1) | (1 << RXM0)); // MCP2515 zurueck in den normalen Modus versetzten mcp2515_write_register(CANCTRL, CLKOUT_PRESCALER_); }
// ---------------------------------------------------------------------------- uint8_t mcp2515_read_status(uint8_t type) { uint8_t data; RESET(MCP2515_CS); spi_putc(type); data = spi_putc(0xff); SET(MCP2515_CS); return data; }
// ------------------------------------------------------------------------ uint8_t term_getc() { uint8_t t; // read databyte RESET(RD); asm ("nop"); t = PIN(USB_DATA); SET(RD); return t; }
// ---------------------------------------------------------------------------- uint8_t mcp2515_get_message(tCAN *message) { // read status uint8_t status = mcp2515_read_status(SPI_RX_STATUS); uint8_t addr; uint8_t t; if (bit_is_set(status,6)) { // message in buffer 0 addr = SPI_READ_RX; } else if (bit_is_set(status,7)) { // message in buffer 1 addr = SPI_READ_RX | 0x04; } else { // Error: no message available return 0; } RESET(MCP2515_CS); spi_putc(addr); // read id message->id = (uint16_t) spi_putc(0xff) << 3; message->id |= spi_putc(0xff) >> 5; spi_putc(0xff); spi_putc(0xff); // read DLC uint8_t length = spi_putc(0xff) & 0x0f; message->header.length = length; message->header.rtr = (bit_is_set(status, 3)) ? 1 : 0; // read data for (t=0;t<length;t++) { message->data[t] = spi_putc(0xff); } SET(MCP2515_CS); // clear interrupt flag if (bit_is_set(status, 6)) { mcp2515_bit_modify(CANINTF, (1<<RX0IF), 0); } else { mcp2515_bit_modify(CANINTF, (1<<RX1IF), 0); } return (status & 0x07) + 1; }
// write either command or data, with automatic 4/8-bit selection void LiquidCrystal::send(unsigned char value, bool mode) { if (this->realTimeDisplay) { if (mode) { SET(_rs_pin); } else { RESET(_rs_pin); } write4bits(value >> 4); pulseEnable(1); write4bits(value); pulseEnable(delayAfterCommand); } else { if (!lcdActions.isFull()) {
int proc_activesource_read(char *page, char **start, off_t off, int count, int *eof, void *data_unused) { unsigned short physAddr = 0; PR_INIT(); physAddr = getActiveSource(); len = sprintf(page, "%01x.%01x.%01x.%01x\n", (physAddr >> 12) & 0xf, (physAddr >> 8) & 0xf, (physAddr >> 4) & 0xf, (physAddr >> 0) & 0xf); RESET(eventActiveSource); PR_EXIT(len) }
static void lcdc_hx8352_config_gpios(void) { /* Setting the Default GPIO's */ spi_sdi = platform_data->gpio_num[0]; spi_sdo = platform_data->gpio_num[1]; lcd_reset = platform_data->gpio_num[2]; spi_cs = platform_data->gpio_num[3]; spi_clk = platform_data->gpio_num[4]; platform_data->panel_config_gpio(GPIO_ENABLE); RESET(1); SCLK(1); CS(1); }
int ft_strcmp(const char *s1, const char *s2) { int a; int b; if (s1 == NULL) return (0); RESET(a, b); while (s1[a] == s2[a] && s1[a] && s2[a]) { a++; } b = s1[a] - s2[a]; return (b); }
// ------------------------------------------------------------------------- uint8_t mcp2515_read_register(uint8_t adress) { uint8_t data; RESET(MCP2515_CS); spi_putc(SPI_READ); spi_putc(adress); data = spi_putc(0xff); SET(MCP2515_CS); return data; }
//reset all the ADS1299's settings. Call however you'd like. Stops all data acquisition void ADS1299::resetADS(void) { RESET(); // send RESET command to default all registers SDATAC(); // exit Read Data Continuous mode to communicate with ADS delay(100); // turn off all channels for (int chan=1; chan <= 8; chan++) { deactivateChannel(chan); } setSRB1(use_SRB1()); //set whether SRB1 is active or not };
void terminal_operation(ringbuf_t *tx, char *op0, char *op1, char *op2, char *op3) { if (!strcmp(op0, "help") || !strcmp(op0, "?")) { ringbuf_put_str(tx, "\tprint_time(t)\n" "\tadjust_time(at) [hand:y,M,d,h,m,s] [value(decimal)]\n" "\tRESET();\n" ); } terminal_time(tx, op0, op1, op2); if (!strcmp(op0, "RESET();")) { RESET(); } if (!strcmp(op0, "bootload")) { asm("goto 0x001C"); } }
void getMotionBurst(uint8_t *burst) { RESET(P_NCS); spi_exch(MOTION_BURST); _delay_us(75); // wait t_SRAD-MOT // read all 7 bytes (Motion, Delta_X, Delta_Y, SQUAL, Shutter_Upper, Shutter_Lower, Maximum Pixels) for(uint8_t i=0; i<7; i++) { burst[i] = spi_exch(0x00); } SET(P_NCS); _delay_us(1); }
void terminal_operation(ringbuf_t *tx, char *op0, char *op1, char *op2, char *op3) { if (!strcmp(op0, "help") || !strcmp(op0, "?")) { ringbuf_put_str(tx, "\t*****************************\n" "\t**** Charge Station 6.12 ****\n" "\t**** Made by @Ryokeri14 ****\n" "\t*****************************\n\n" "\tprint_time(t)\n" "\tadjust_time(at) [hand:y,M,d,h,m,s] [value(decimal)]\n" "\tsettings(set) [port:left(l),center(c),right(r)]\n\t\t[auto_stop(as),always_on(ao),integrating_reset(ir)]\n" "\toutput_data(od) <- stopped by pressing the Enter key\n" "\ttouch [reset(r),initialize(init)] [value(%)]\n" "\n\tfor Developper\n" "\tRESET();\n" "\tbootload\n" ); } terminal_time(tx, op0, op1, op2); if (!strcmp(op0, "RESET();")) { RESET(); } if (!strcmp(op0, "bootload")) { bootload(); } if (!strcmp(op0, "od") || !strcmp(op0, "output_data")) { terminal_data_out = 1; } if (!strcmp(op0, "")) { terminal_data_out = 0; } if (!strcmp(op0, "touch")) { if (!strcmp(op1, "initialize") || !strcmp(op1, "init")) { uint8_t percentage = atoi(op2); if (percentage <= 100) { ctmu_ratio = percentage; ctmu_set_ratio(ctmu_ratio); ringbuf_put_str(tx, "\tinitializing complete\n"); } else { ringbuf_put_str(tx, "\tinvalid data\n\tinitializing failed\n"); } } if (!strcmp(op1, "reset") || !strcmp(op1, "r")) { ctmu_set_ratio(ctmu_ratio); ringbuf_put_str(tx, "\tseting complete\n"); } } }
// ------------------------------------------------------------------------ void term_putc(const char c) { // wait until ft245 ready to receive byte while (IS_SET(USB_TXE)) ; DDR(USB_DATA) = 0xff; PORT(USB_DATA) = c; // write data SET(WR); asm ("nop"); RESET(WR); // use port as input, pull-ups on DDR(USB_DATA) = 0; PORT(USB_DATA) = 0xff; }
uint8_t read(uint8_t address) { RESET(P_NCS); spi_exch(address); if (address == 0x02) // is motion read? { _delay_us(75); // wait t_SRAD-MOT } else { _delay_us(50); // wait t_SRAD } uint8_t ret = spi_exch(0x00); SET(P_NCS); _delay_us(1); // wait t_SRR = 250ns return ret; }
/* Try to expand each nonprime and noncovered cube */ foreach_set(F, last, p) { /* do not expand if PRIME or if covered by previous expansion */ if (! TESTP(p, PRIME) && ! TESTP(p, COVERED)) { /* expand the cube p, result is RAISE */ expand1(R, F, RAISE, FREESET, OVEREXPANDED_CUBE, SUPER_CUBE, INIT_LOWER, &num_covered, p); if (debug & EXPAND) printf("EXPAND: %s (covered %d)\n", pc1(p), num_covered); (void) set_copy(p, RAISE); SET(p, PRIME); RESET(p, COVERED); /* not really necessary */ /* See if we generated an inessential prime */ if (num_covered == 0 && ! setp_equal(p, OVEREXPANDED_CUBE)) { SET(p, NONESSEN); } } }
void start_group (FILE **StartFile, int ***auts, struct pga_vars *pga, struct pcp_vars *pcp) { register int *y = y_address; int retain; int ***central; *StartFile = TemporaryFile (); save_pcp (*StartFile, pcp); retain = pcp->lastg; pcp->lastg = y[pcp->clend + pcp->cc - 1]; pga->nmr_stabilisers = pga->m; pga->nmr_centrals = 0; pga->final_stage = TRUE; set_values (pga, pcp); save_pga (*StartFile, central, auts, pga, pcp); RESET(*StartFile); pcp->lastg = retain; }
void Verbose::generateReplacementString(const CombinatorPtr& term1, const CombinatorPtr& term2) { if (!isVerbose()) return; QString from = term1->toString(); QString to = term2->toString(); if (from.isEmpty() || to.isEmpty()) return; *m_stream << " " << prefix() << YELLOW() << from << "->" << to << RESET() << postfix() << "\n"; print(); }
void terminal_operation(ringbuf_t *tx, char *op0, char *op1, char *op2, char *op3) { if (!strcmp(op0, "help") || !strcmp(op0, "?")) { ringbuf_put_str(tx, "\t******************************\n" "\t**** PIC18F27J53 template ****\n" "\t**** Made by @Ryokeri14 ****\n" "\t******************************\n\n" "\tprint_time(t)\n" "\tadjust_time(at) [hand:y,M,d,h,m,s] [value(decimal)]\n" "\n\tfor Developper\n" "\tRESET();\n" "\tbootload\n" ); } terminal_time(tx, op0, op1, op2); if (!strcmp(op0, "RESET();")) { RESET(); } if (!strcmp(op0, "bootload")) { asm("goto 0x001C"); } }
int HitBreakSprite ( short BreakSprite, short type ) { SPRITEp sp; SPRITEp bp = &sprite[BreakSprite]; USERp bu = User[BreakSprite]; short match = bp->lotag; short match_extra; short SpriteNum; BREAK_INFOp break_info; // ignore as a breakable if true //if (sp->lotag == TAG_SPRITE_HIT_MATCH) // return(FALSE); if ( TEST_BOOL1 ( bp ) ) { if ( TEST_BOOL2 ( bp ) ) { return ( FALSE ); } return ( UserBreakSprite ( BreakSprite ) ); } if ( bu && !NullActor ( bu ) ) { // programmed animating type - without BOOL1 set if ( bp->lotag ) { DoLightingMatch ( bp->lotag, -1 ); } SpawnShrap ( BreakSprite, -1 ); RESET ( bp->extra, SPRX_BREAKABLE ); return ( FALSE ); } return ( AutoBreakSprite ( BreakSprite, type ) ); }
void getFrameBurst(uint8_t *image) { uint8_t regValue; write(FRAME_CAPTURE,0x83); // wait 3 frame periods + 10 us for frame to be captured // min frame speed is 2000 frames/second so 1 frame = 500 us. so 500 x 3 + 10 = 1510 _delay_us(1510); // wait t_CAPTURE RESET(P_NCS); spi_exch(PIXEL_BURST); _delay_us(50); // wait t_SRAD for(int i=0; i<900; i++) // max. 1536 Pixels { regValue = spi_exch(0x00); image[i] = (regValue << 2); _delay_us(10); // wait t_LOAD } SET(P_NCS); _delay_us(4); // wait t_BEXIT }
/*f * Throw away any typeahead that has been typed by the user * and has not yet been read by the program. */ int flushinp() { int fd; #ifdef M_CURSES_TRACE __m_trace("flushinp(void)"); #endif if (!ISEMPTY()) RESET(); if (cur_term->_flags & __TERM_ISATTY_IN) fd = cur_term->_ifd; else if (cur_term->_flags & __TERM_ISATTY_OUT) fd = cur_term->_ofd; else fd = -1; if (0 <= fd) (void) tcflush(fd, TCIFLUSH); return __m_return_code("flushinp", OK); }
void Temp::load(int32_t &addr, int id) { R0 = read_float(addr); R1 = read_float(addr); logRc = read_float(addr); Tc = read_float(addr); beta = read_float(addr); K = exp(logRc - beta / Tc); //debug("K %f R0 %f R1 %f logRc %f Tc %f beta %f", K, R0, R1, logRc, Tc, beta); /* core_C = read_float(addr); shell_C = read_float(addr); transfer = read_float(addr); radiation = read_float(addr); power = read_float(addr); */ power_pin[0].read(read_16(addr)); power_pin[1].read(read_16(addr)); int old_pin = thermistor_pin.write(); bool old_valid = thermistor_pin.valid(); thermistor_pin.read(read_16(addr)); target[1] = read_float(addr); arch_set_duty(power_pin[1], read_float(addr)); for (int i = 0; i < 2; ++i) { adctarget[i] = toadc(target[i], MAXINT); SET_OUTPUT(power_pin[i]); if (is_on[i]) SET(power_pin[i]); else RESET(power_pin[i]); } if (old_pin != thermistor_pin.write() && old_valid) arch_setup_temp(~0, old_pin, false); if (thermistor_pin.valid()) arch_setup_temp(id, thermistor_pin.pin, true, power_pin[0].valid() ? power_pin[0].pin : ~0, power_pin[0].inverted(), adctarget[0], power_pin[1].valid() ? power_pin[1].pin : ~0, power_pin[1].inverted(), adctarget[1]); }
void interrupt low_priority SlowTick() { unsigned int uiTemp; static unsigned char RotaryEncoder = 0; static signed char RawRotaryEncoder = 0; static unsigned char RotaryTickInterval = 0; static unsigned char RotaryDetentIntervalIndex = 0; static unsigned int SwitchPressDuration = 0; static unsigned int internal_BatteryVoltage = 1000; static unsigned int LCD_IdleTimer = 0; static unsigned long internal_Tick500Hz = 0; static signed long internal_PID_SetPoint; static signed int PID_LastError; static signed long LastMotorPosition = 0; static bit isPendingUIEvent = 0; static signed char PendingUIEvent = 0; static bit isPendingRotaryTick = 0; static unsigned int PendingRotaryTick = 0; signed int MotorSpeed; signed int ResponseOutput; signed int PID_Error_delta; signed long PID_Error; signed long CurrentMotorPosition; unsigned long PID_ResponseLimit = 0; TMR1H = 0x83; TMR1L = 0x1A; TMR1IF = 0; //--------------------------------------------------------------------------- //Timer Functions //--------------------------------------------------------------------------- internal_Tick500Hz++; if (!bLock_Tick500Hz) Tick500Hz = internal_Tick500Hz; if (bWaiting) { if (internal_Tick500Hz > WaitUntil_Tick500Hz) { bWaiting = 0; } } //--------------------------------------------------------------------------- //LCD Backlight PWM //--------------------------------------------------------------------------- if (LCD_IdleTimer) { LCD_IdleTimer--; if (bPowerOff) LCD_BacklightEnabled = 0; else LCD_BacklightEnabled = 1; } else { LCD_BacklightEnabled = 0; } //--------------------------------------------------------------------------- //Process the Rotary Input //--------------------------------------------------------------------------- RotaryEncoder <<= 2; //remember previous state RotaryEncoder |= UI_ENCODER; //add current state RotaryEncoder & = 0x0F; RawRotaryEncoder -= enc_states[RotaryEncoder]; RotaryTickInterval++; if (RotaryTickInterval == 0xFF) { RotaryTickInterval = 0; RawRotaryEncoder = 0; ZeroRotaryDetentIntervals = 1; } if (RawRotaryEncoder > 2) { PendingUIEvent = USER_INPUT_INC; isPendingUIEvent = 1; LCD_IdleTimer = Config.BacklightIdleTimeout; isPendingRotaryTick = 1; PendingRotaryTick = RotaryTickInterval; RotaryTickInterval = 0; RawRotaryEncoder = 0; } if (RawRotaryEncoder<-2) { PendingUIEvent = USER_INPUT_DEC; isPendingUIEvent = 1; LCD_IdleTimer = Config.BacklightIdleTimeout; isPendingRotaryTick = 1; PendingRotaryTick = RotaryTickInterval; RotaryTickInterval = 0; RawRotaryEncoder = 0; } //--------------------------------------------------------------------------- //Process the Rotary Input Switch //--------------------------------------------------------------------------- if (UI_SWITCH) { SwitchPressDuration++; LCD_IdleTimer = Config.BacklightIdleTimeout; } else { if ((SwitchPressDuration > 1) & (SwitchPressDuration < BACK_DURATION)) { PendingUIEvent = USER_INPUT_CLICK; isPendingUIEvent = 1; } SwitchPressDuration = 0; } //if (SwitchPressDuration > RESET_DURATION) SwitchPressDuration = RESET_DURATION; if (SwitchPressDuration > RESET_DURATION) RESET(); if (SwitchPressDuration == CANCEL_DURATION) { PendingUIEvent = USER_INPUT_CANCEL; isPendingUIEvent = 1; } if (SwitchPressDuration == BACK_DURATION) { PendingUIEvent = USER_INPUT_BACK; isPendingUIEvent = 1; } if (ClearUI_Event) { isPendingUIEvent = 0; PendingUIEvent = 0; UI_Event = 0; ClearUI_Event = 0; bLock_UI_Event = 0; } if (!bLock_UI_Event) { if (isPendingUIEvent) { UI_Event = PendingUIEvent; isPendingUIEvent = 0; } if (isPendingExternalUIEvent) { isPendingExternalUIEvent = 0; UI_Event = PendingExternalUIEvent; } } if (!bLock_RotaryDetentIntervals) { if (isPendingRotaryTick) { RotaryDetentIntervals[RotaryDetentIntervalIndex] = PendingRotaryTick; RotaryDetentIntervalIndex++; if (RotaryDetentIntervalIndex > 15) RotaryDetentIntervalIndex = 0; isPendingRotaryTick = 0; } } //--------------------------------------------------------------------------- //Grab the Current Motor's Position in an Interrupt Safe Way //--------------------------------------------------------------------------- bLock_Motor_Position = 1; CurrentMotorPosition = Motor_Position; bLock_Motor_Position = 0; //--------------------------------------------------------------------------- //Process the Motion Trajectory Engine //--------------------------------------------------------------------------- if (bMove_InProgress) { bFollowMode = 0; if (Move_shifted_position.ul > Move_DecelPosition) { Move_speedQ24 -= Move_AccelValueQ24; if (Move_speedQ24 < Move_AccelValueQ24) { internal_PID_SetPoint = Move_FinalPosition; Move_speedQ24 = 0; bMove_InProgress = 0; } } else { if (Move_speedQ24 < Move_CoastSpeedQ24) { Move_speedQ24 += Move_AccelValueQ24; if (Move_speedQ24 > Move_CoastSpeedQ24) Move_speedQ24 = Move_CoastSpeedQ24; } if (Move_speedQ24 > Move_CoastSpeedQ24) { Move_speedQ24 -= Move_AccelValueQ24; if (Move_speedQ24 < Move_CoastSpeedQ24) Move_speedQ24 = Move_CoastSpeedQ24; } } Move_position[0].ul += Move_speedQ24; if (Move_position[0].ul < Move_speedQ24) Move_position[1].ul++; Move_shifted_position.ub[0] = Move_position[0].ub[3]; Move_shifted_position.ub[1] = Move_position[1].ub[0]; Move_shifted_position.ub[2] = Move_position[1].ub[1]; Move_shifted_position.ub[3] = Move_position[1].ub[2]; if (bMove_InProgress) { if (bMove_Neg) internal_PID_SetPoint = Move_Origin - Move_shifted_position.ul; else internal_PID_SetPoint = Move_Origin + Move_shifted_position.ul; } } else if (bFollowMode) { PID_Error = internal_PID_SetPoint - CurrentMotorPosition; internal_PID_SetPoint = CurrentMotorPosition; //internal_PID_SetPoint -= (PID_Error >> 1); } else if (bSpeedMode) { if (Speed_SetPending) { Speed_SetPending = 0; Move_CoastSpeedQ24 = Speed_SetToThis; } if (Move_speedQ24 < Move_CoastSpeedQ24) { Speed_IsAccelerating = 1; Move_speedQ24 += Move_AccelValueQ24; if (Move_speedQ24 > Move_CoastSpeedQ24) Move_speedQ24 = Move_CoastSpeedQ24; } else if (Move_speedQ24 > Move_CoastSpeedQ24) { Speed_IsAccelerating = 1; if (Move_speedQ24 < Move_AccelValueQ24) Move_speedQ24 = 0; else Move_speedQ24 -= Move_AccelValueQ24; if (Move_speedQ24 < Move_CoastSpeedQ24) Move_speedQ24 = Move_CoastSpeedQ24; } else Speed_IsAccelerating = 0; Move_position[0].ul += Move_speedQ24; if (Move_position[0].ul < Move_speedQ24) Move_position[1].ul++; Move_shifted_position.ub[0] = Move_position[0].ub[3]; Move_shifted_position.ub[1] = Move_position[1].ub[0]; Move_shifted_position.ub[2] = Move_position[1].ub[1]; Move_shifted_position.ub[3] = Move_position[1].ub[2]; /* TX_Idx=0; TXBuffer[TX_Idx++]=Move_shifted_position.ub[0]; TXBuffer[TX_Idx++]=Move_shifted_position.ub[1]; TXBuffer[TX_Idx++]=Move_shifted_position.ub[2]; TXBuffer[TX_Idx++]=Move_shifted_position.ub[3]; TX_Idx = 0; TX_bCount = 4;*/ if (bMove_Neg) internal_PID_SetPoint = Move_Origin - Move_shifted_position.ul; else internal_PID_SetPoint = Move_Origin + Move_shifted_position.ul; } //--------------------------------------------------------------------------- //Report back the PID Setpoint in a interrupt safe manner. //--------------------------------------------------------------------------- //if (!bLock_PID_SetPoint) PID_SetPoint = internal_PID_SetPoint; //--------------------------------------------------------------------------- //Read the Battery Level which is filtered simply by single incrementing. It will take ~2000 cycles to get the real battery value... //Check for low battery voltage trips //--------------------------------------------------------------------------- uiTemp = ADRESH; uiTemp <<= 8; uiTemp += ADRESL; GO = 1; //Start the next conversion signed int diff = uiTemp - internal_BatteryVoltage; diff >>= 4; internal_BatteryVoltage += diff; if (uiTemp > internal_BatteryVoltage) internal_BatteryVoltage++; if (uiTemp < internal_BatteryVoltage) internal_BatteryVoltage--; if (!bLock_BatteryVoltage) BatteryVoltage = internal_BatteryVoltage; if (internal_BatteryVoltage < Config.BatteryLowVoltageLevel) bLowVoltageTrip = 1; //--------------------------------------------------------------------------- //Figure out the Power Scaling... //--------------------------------------------------------------------------- MotorSpeed = LastMotorPosition - CurrentMotorPosition; LastMotorPosition = CurrentMotorPosition; if (MotorSpeed < 0) MotorSpeed = -MotorSpeed; //First we get a linear scale from 1Tick/Count=50% power to 23Ticks/Count=100% power //The resulting Limit is from 128 to 256 uiTemp = (MotorSpeed * Config.PowerResponseLimiter_Slope); uiTemp += Config.PowerResponseLimiter_Intercept; if (uiTemp > 0xFF) uiTemp = 0xFF; //Now use Fixed-Point Math to calculate the battery voltage scaling //A 10V battery would require a higher PWM value to power the motor with the right voltage than a 15V Battery unsigned char ucBatt = (internal_BatteryVoltage >> 4); unsigned int BatteryScaleQ8 = Config.Battery12V_Q8 / ucBatt; PID_ResponseLimit = uiTemp; PID_ResponseLimit *= BatteryScaleQ8; PID_ResponseLimit >>= 6; if (PID_ResponseLimit > 1023) PID_ResponseLimit = 1023; PID_ResponseLimit = 1023; //--------------------------------------------------------------------------- //Calculate the PID Error //--------------------------------------------------------------------------- PID_Error = internal_PID_SetPoint - CurrentMotorPosition; if (PID_Error > Config.PID_MaxError) { PID_Error = Config.PID_MaxError; bMotionError = 1; } else if (PID_Error<-Config.PID_MaxError) { PID_Error = -Config.PID_MaxError; bMotionError = 1; } //--------------------------------------------------------------------------- //Calculate the PID Response //--------------------------------------------------------------------------- PID_Error_delta = PID_Error - PID_LastError; PID_LastError = PID_Error; ResponseOutput = (long) Config.PID_Kp * (long) PID_Error; ResponseOutput += (long) Config.PID_Kd * (long) PID_Error_delta; //--------------------------------------------------------------------------- //Set the PWM Output and Direction //--------------------------------------------------------------------------- if (ResponseOutput < 0) { MOTOR_FORWARD = 0; MOTOR_REVERSE = 1; ResponseOutput = -ResponseOutput; } else { MOTOR_FORWARD = 1; MOTOR_REVERSE = 0; } if (ResponseOutput > PID_ResponseLimit) ResponseOutput = PID_ResponseLimit; if (bPowerOff) { MOTOR_FORWARD = 0; MOTOR_REVERSE = 0; ResponseOutput = 0; internal_PID_SetPoint = CurrentMotorPosition; } //Enforce the Minimum Off Time Requirements for the PWM Driver IC (6us Min) //As the time on increases the time off decreases. So once we get above a certain //level we need to skip righ to 100% on. if (ResponseOutput > 900) ResponseOutput = 0x3FF; //Invert the output due to the NPN transistors. ResponseOutput = 0x3FF - ResponseOutput; //Set the PWM Duty Cycle MSB's ResponseOutput >>= 2; MOTOR_PWM_H = (unsigned char) ResponseOutput; }