void motorRightOff(void){
chSysLock();
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
chBSemSignalI(&motor_right_sem);
chSysUnlock();
}
/*
* ADC end conversion callback.
* The PWM channels are reprogrammed using the latest ADC samples.
* The latest samples are transmitted into a single SPI transaction.
*/
void adccb(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
(void) buffer; (void) n;
/* Note, only in the ADC_COMPLETE state because the ADC driver fires an
intermediate callback when the buffer is half full.*/
if (adcp->state == ADC_COMPLETE) {
adcsample_t avg_ch1, avg_ch2;
/* Calculates the average values from the ADC samples.*/
avg_ch1 = (samples[0] + samples[2] + samples[4] + samples[6]) / 4;
avg_ch2 = (samples[1] + samples[3] + samples[5] + samples[7]) / 4;
chSysLockFromIsr();
/* Changes the channels pulse width, the change will be effective
starting from the next cycle.*/
pwmEnableChannelI(&PWMD4, 0, PWM_FRACTION_TO_WIDTH(&PWMD4, 4096, avg_ch1));
pwmEnableChannelI(&PWMD4, 3, PWM_FRACTION_TO_WIDTH(&PWMD4, 4096, avg_ch2));
/* SPI slave selection and transmission start.*/
spiSelectI(&SPID2);
spiStartSendI(&SPID2, ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH, samples);
chSysUnlockFromIsr();
}
}
void motor_right_off(void *p) {
(void) p;
chSysLockFromIsr();
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
chBSemSignalI(&motor_right_sem);
chSysUnlockFromIsr();
}
void motor_right_on(Direction dir){
chBSemWait(&motor_right_sem);
chSysLock();
if(dir == D_FORWARD){
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,motor_speed));
}else{
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,motor_speed));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
}
chSysUnlock();
}
/**
* @brief Write bit routine.
* @details Switch PWM channel to 'width' or 'narrow' pulse depending
* on value of bit need to be transmitted.
*
* @param[in] owp pointer to the @p onewireDriver object
* @param[in] bit value to be written
*
* @notapi
*/
static void ow_write_bit_I(onewireDriver *owp, ioline_t bit) {
#if ONEWIRE_SYNTH_SEARCH_TEST
_synth_ow_write_bit(owp, bit);
#else
osalSysLockFromISR();
if (0 == bit) {
pwmEnableChannelI(owp->config->pwmd, owp->config->master_channel,
ONEWIRE_ZERO_WIDTH);
}
else {
pwmEnableChannelI(owp->config->pwmd, owp->config->master_channel,
ONEWIRE_ONE_WIDTH);
}
osalSysUnlockFromISR();
#endif
}
/**
* @brief Read some bites from slave device.
*
* @param[in] owp pointer to the @p onewireDriver object
* @param[out] rxbuf pointer to the buffer for read data
* @param[in] rxbytes amount of data to be received
*/
void onewireRead(onewireDriver *owp, uint8_t *rxbuf, size_t rxbytes) {
PWMDriver *pwmd;
PWMConfig *pwmcfg;
size_t mch, sch;
osalDbgCheck((NULL != owp) && (NULL != rxbuf));
osalDbgCheck((rxbytes > 0) && (rxbytes <= ONEWIRE_MAX_TRANSACTION_LEN));
osalDbgAssert(owp->reg.state == ONEWIRE_READY, "Invalid state");
/* Buffer zeroing. This is important because of driver collects
bits using |= operation.*/
memset(rxbuf, 0, rxbytes);
pwmd = owp->config->pwmd;
pwmcfg = owp->config->pwmcfg;
mch = owp->config->master_channel;
sch = owp->config->sample_channel;
owp->reg.bit = 0;
owp->reg.final_timeslot = false;
owp->buf = rxbuf;
owp->reg.bytes = rxbytes;
pwmcfg->period = ONEWIRE_ZERO_WIDTH + ONEWIRE_RECOVERY_WIDTH;
pwmcfg->callback = NULL;
pwmcfg->channels[mch].callback = NULL;
pwmcfg->channels[mch].mode = owp->config->pwmmode;
pwmcfg->channels[sch].callback = pwm_read_bit_cb;
pwmcfg->channels[sch].mode = PWM_OUTPUT_ACTIVE_LOW;
ow_bus_active(owp);
osalSysLock();
pwmEnableChannelI(pwmd, mch, ONEWIRE_ONE_WIDTH);
pwmEnableChannelI(pwmd, sch, ONEWIRE_SAMPLE_WIDTH);
pwmEnableChannelNotificationI(pwmd, sch);
osalThreadSuspendS(&owp->thread);
osalSysUnlock();
ow_bus_idle(owp);
}
/**
* @brief Generate reset pulse on bus.
*
* @param[in] owp pointer to the @p onewireDriver object
*
* @return Bool flag denoting device presence.
* @retval true There is at least one device on bus.
*/
bool onewireReset(onewireDriver *owp) {
PWMDriver *pwmd;
PWMConfig *pwmcfg;
size_t mch, sch;
osalDbgCheck(NULL != owp);
osalDbgAssert(owp->reg.state == ONEWIRE_READY, "Invalid state");
/* short circuit on bus or any other device transmit data */
if (PAL_LOW == ow_read_bit(owp))
return false;
pwmd = owp->config->pwmd;
pwmcfg = owp->config->pwmcfg;
mch = owp->config->master_channel;
sch = owp->config->sample_channel;
pwmcfg->period = ONEWIRE_RESET_LOW_WIDTH + ONEWIRE_RESET_SAMPLE_WIDTH;
pwmcfg->callback = NULL;
pwmcfg->channels[mch].callback = NULL;
pwmcfg->channels[mch].mode = owp->config->pwmmode;
pwmcfg->channels[sch].callback = pwm_reset_cb;
pwmcfg->channels[sch].mode = PWM_OUTPUT_ACTIVE_LOW;
ow_bus_active(owp);
osalSysLock();
pwmEnableChannelI(pwmd, mch, ONEWIRE_RESET_LOW_WIDTH);
pwmEnableChannelI(pwmd, sch, ONEWIRE_RESET_SAMPLE_WIDTH);
pwmEnableChannelNotificationI(pwmd, sch);
osalThreadSuspendS(&owp->thread);
osalSysUnlock();
ow_bus_idle(owp);
/* wait until slave release bus to discriminate short circuit condition */
osalThreadSleepMicroseconds(500);
return (PAL_HIGH == ow_read_bit(owp)) && (true == owp->reg.slave_present);
}
/**
* @brief Enables a PWM channel.
* @pre The PWM unit must have been activated using @p pwmStart().
* @post The channel is active using the specified configuration.
* @note Depending on the hardware implementation this function has
* effect starting on the next cycle (recommended implementation)
* or immediately (fallback implementation).
*
* @param[in] pwmp pointer to a @p PWMDriver object
* @param[in] channel PWM channel identifier (0...channels-1)
* @param[in] width PWM pulse width as clock pulses number
*
* @api
*/
void pwmEnableChannel(PWMDriver *pwmp,
pwmchannel_t channel,
pwmcnt_t width) {
osalDbgCheck((pwmp != NULL) && (channel < pwmp->channels));
osalSysLock();
osalDbgAssert(pwmp->state == PWM_READY, "not ready");
pwmEnableChannelI(pwmp, channel, width);
osalSysUnlock();
}
static void contAdcReadyCb( ADCDriver * adcp, adcsample_t * buffer, size_t n )
{
(void)adcp;
//(void)buffer;
(void)n;
// Buck
if ( buffer[ BUCK_IND ] < buckSp )
{
buckPwm += buckGain;
buckPwm = ( buckPwm <= 10000 ) ? buckPwm : 10000;
pwmEnableChannelI(&CONV_PWM, PWM_BUCK_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, buckPwm ) );
}
else if ( buffer[ BUCK_IND ] > buckSp )
{
buckPwm = ( buckPwm >= buckGain ) ? buckPwm : buckGain;
buckPwm -= buckGain;
pwmEnableChannelI(&CONV_PWM, PWM_BUCK_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, buckPwm ) );
}
// Boost
if ( buffer[ INPUT_IND ] >= inputSp )
{
if ( buffer[ BOOST_IND ] < boostSp )
{
boostPwm += boostGain;
boostPwm = ( boostPwm <= 9000 ) ? boostPwm : 9000;
pwmEnableChannelI(&CONV_PWM, PWM_BOOST_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, boostPwm ) );
}
else if ( buffer[ BOOST_IND ] > boostSp )
{
boostPwm = ( boostPwm >= boostGain ) ? boostPwm : boostGain;
boostPwm -= boostGain;
pwmEnableChannelI(&CONV_PWM, PWM_BOOST_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, boostPwm ) );
}
}
else
pwmEnableChannelI(&CONV_PWM, PWM_BOOST_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, 0 ) );
};
/******************************************************************************
* Function name: ControlAxis
*
* Description: Updates PWM outputs based on system state
*****************************************************************************/
void ControlAxis(CONTROL_AXIS_STRUCT *axis_p, adcsample_t sample, uint8_t PWM_U_CHAN, uint8_t PWM_V_CHAN) {
//void ControlAxis(CONTROL_AXIS_STRUCT *axis_p, uint8_t PWM_U_CHAN, uint8_t PWM_V_CHAN) {
// Get feedback ADC value
axis_p->U16FeedbackADC = sample;
// // Get commanded position
// if(U8trackupflag) {
// TrackUp();
// }
// Set axis mode
if(U8ShellMode == 2)
axis_p->U8PosnVelMode = U8PosnVelModeSwitch;
else
axis_p->U8PosnVelMode = U8ShellMode;
// Run control loop
controlLoop(axis_p);
chSysLockFromIsr();
// Set axis PWM's
if(axis_p->S16OutputCommand > 0){ //Forward
// Compute ON time scaled 0 - 511 counts
// Can't go to 100% DC, so max is 98%, or 958
u32Temp = ((uint32_t)(axis_p->S16OutputCommand * (2 * 958))) / 1000;
pwmEnableChannelI(&PWMD4, PWM_U_CHAN - 1, microsecondsToPWMTicks( u32Temp )); // U-Pole
pwmEnableChannelI(&PWMD4, PWM_V_CHAN - 1, microsecondsToPWMTicks( 0 )); // V-Pole
}else{ // Reverse
// Compute ON time scaled 0 - 511 counts
// Can't go to 100% DC, so max is 98%, or 958
u32Temp = ((uint32_t)(axis_p->S16OutputCommand * (-2 * 958))) / 1000;
pwmEnableChannelI(&PWMD4, PWM_U_CHAN - 1, microsecondsToPWMTicks( 0 )); // U-Pole
pwmEnableChannelI(&PWMD4, PWM_V_CHAN - 1, microsecondsToPWMTicks( u32Temp )); // V-Pole
}
chSysUnlockFromIsr();
}
void PwmBacklight::pwmcb(PWMDriver * pwm)
{
if (BCurrent == BFinish)
return;
if (BDirection == B_UP)
{
BCurrent++;
}
else
{
BCurrent--;
}
pwmEnableChannelI(pwm,0,BCurrent * 10);
}
static msg_t Thread1(void *arg) {
(void)arg;
int val = 0;
int val_dir = 1;
chRegSetThreadName("counter");
while (TRUE) {
palTogglePad(IOPORT2, GPIOB_LED);
chThdSleepMilliseconds(10);
pwmEnableChannelI(&PWMD3, 3, PWM_FRACTION_TO_WIDTH(&PWMD3, 100, val));
val += val_dir;
if(val < 0) { val = 0; val_dir = 1; }
if(val > 100) { val = 100; val_dir = -1; }
}
return 0;
}
/**
* @brief Performs tree search on bus.
* @note This function does internal 1-wire reset calls every search
* iteration.
*
* @param[in] owp pointer to a @p OWDriver object
* @param[out] result pointer to buffer for discovered ROMs
* @param[in] max_rom_cnt buffer size in ROMs count for overflow prevention
*
* @return Count of discovered ROMs. May be more than max_rom_cnt.
* @retval 0 no ROMs found or communication error occurred.
*/
size_t onewireSearchRom(onewireDriver *owp, uint8_t *result,
size_t max_rom_cnt) {
PWMDriver *pwmd;
PWMConfig *pwmcfg;
uint8_t cmd;
size_t mch, sch;
osalDbgCheck(NULL != owp);
osalDbgAssert(ONEWIRE_READY == owp->reg.state, "Invalid state");
osalDbgCheck((max_rom_cnt <= 256) && (max_rom_cnt > 0));
pwmd = owp->config->pwmd;
pwmcfg = owp->config->pwmcfg;
cmd = ONEWIRE_CMD_SEARCH_ROM;
mch = owp->config->master_channel;
sch = owp->config->sample_channel;
search_clean_start(&owp->search_rom);
do {
/* every search must be started from reset pulse */
if (false == onewireReset(owp))
return 0;
/* initialize buffer to store result */
if (owp->search_rom.reg.devices_found >= max_rom_cnt)
owp->search_rom.retbuf = result + 8*(max_rom_cnt-1);
else
owp->search_rom.retbuf = result + 8*owp->search_rom.reg.devices_found;
memset(owp->search_rom.retbuf, 0, 8);
/* clean iteration state */
search_clean_iteration(&owp->search_rom);
/**/
onewireWrite(&OWD1, &cmd, 1, 0);
/* Reconfiguration always needed because of previous call onewireWrite.*/
pwmcfg->period = ONEWIRE_ZERO_WIDTH + ONEWIRE_RECOVERY_WIDTH;
pwmcfg->callback = NULL;
pwmcfg->channels[mch].callback = NULL;
pwmcfg->channels[mch].mode = owp->config->pwmmode;
pwmcfg->channels[sch].callback = pwm_search_rom_cb;
pwmcfg->channels[sch].mode = PWM_OUTPUT_ACTIVE_LOW;
ow_bus_active(owp);
osalSysLock();
pwmEnableChannelI(pwmd, mch, ONEWIRE_ONE_WIDTH);
pwmEnableChannelI(pwmd, sch, ONEWIRE_SAMPLE_WIDTH);
pwmEnableChannelNotificationI(pwmd, sch);
osalThreadSuspendS(&owp->thread);
osalSysUnlock();
ow_bus_idle(owp);
if (ONEWIRE_SEARCH_ROM_ERROR != owp->search_rom.reg.result) {
/* check CRC and return 0 (0 == error) if mismatch */
if (owp->search_rom.retbuf[7] != onewireCRC(owp->search_rom.retbuf, 7))
return 0;
/* store cached result for usage in next iteration */
memcpy(owp->search_rom.prev_path, owp->search_rom.retbuf, 8);
}
}
while (ONEWIRE_SEARCH_ROM_SUCCESS == owp->search_rom.reg.result);
/**/
if (ONEWIRE_SEARCH_ROM_ERROR == owp->search_rom.reg.result)
return 0;
else
return owp->search_rom.reg.devices_found;
}
