/* * Sets the value of OCnRS for the specified hbridge. */ static void set_speed(uint8_t hbridge_id, uint32_t speed) { enum motor_list motor = (enum motor_list)hbridge_id; if (motor < NUM_MOTORS) { switch(Motors[motor].ocn) { case 1: SetDCOC1PWM(speed); break; case 2: SetDCOC2PWM(speed); break; case 3: SetDCOC3PWM(speed); break; case 4: SetDCOC4PWM(speed); break; case 5: SetDCOC5PWM(speed); break; default: break; } } }
void setDutyCycle(int outputControlX, int dutyCycle) { switch(outputControlX) { case 1: SetDCOC1PWM(dutyCycle); break; case 2: SetDCOC2PWM(dutyCycle); break; case 3: SetDCOC3PWM(dutyCycle); break; case 4: SetDCOC4PWM(dutyCycle); break; case 5: SetDCOC5PWM(dutyCycle); break; default: SetDCOC1PWM(dutyCycle); break; } }
void __ISR(_I2C_1_VECTOR, ipl3) _SlaveI2CHandler(void) { unsigned char temp; static unsigned int dIndex; // check for MASTER and Bus events and respond accordingly if (IFS1bits.I2C1MIF) { mI2C1MClearIntFlag(); return; } if (IFS1bits.I2C1BIF) {//bus collision, reset I2C state machine I2Cstate = 0; mI2C1BClearIntFlag(); return; } // handle the incoming message if ((I2C1STATbits.R_W == 0) && (I2C1STATbits.D_A == 0)) { // reset any state variables needed by a message sequence // perform a dummy read temp = SlaveReadI2C1(); I2C1CONbits.SCLREL = 1; // release the clock I2Cstate = 0; } else if ((I2C1STATbits.R_W == 0) && (I2C1STATbits.D_A == 1)) {//data received, input to slave WDTCount = 0; WriteTimer1(0); // writing data to our module I2CDataIn = SlaveReadI2C1(); I2C1CONbits.SCLREL = 1; // release clock stretch bit if (I2Cstate == 0 && I2CDataIn != ADDR_CLR_I2C_STATE) { I2Cstate = 1; I2C_request = I2CDataIn; } else if (I2Cstate == 1) { switch (I2C_request) { case ADDR_M3_PWM: M3_Dir = I2CDataIn >> 7 & 0x01;//take top bit if (((Status2 & M3_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) { M3_PWM = 0; } else { if (I2CDataIn & 0x80) { mPORTBClearBits(BIT_13); M3_PWM = (((int) ((~I2CDataIn) + 1)) << 4); } else { mPORTBSetBits(BIT_13); M3_PWM = ((int) I2CDataIn) << 4; } } SetDCOC4PWM(M3_PWM); break; case ADDR_M4_PWM: M4_Dir = I2CDataIn >> 7 & 0x01;//take top bit if (((Status2 & M4_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) { M4_PWM = 0; } else { if (I2CDataIn & 0x80) { mPORTBClearBits(BIT_14); M4_PWM = (((int) ((~I2CDataIn) + 1)) << 4); } else { mPORTBSetBits(BIT_14); M4_PWM = ((int) I2CDataIn) << 4; } } SetDCOC5PWM(M4_PWM); break; case ADDR_M5_PWM: M5_Dir = I2CDataIn >> 7 & 0x01;//take top bit if (((Status2 & M5_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) { M5_PWM = 0; } else { if (I2CDataIn & 0x80) { mPORTBClearBits(BIT_15); M5_PWM = (((int) ((~I2CDataIn) + 1)) << 4); } else { mPORTBSetBits(BIT_15); M5_PWM = ((int) I2CDataIn) << 4; } } SetDCOC3PWM(M5_PWM); break; case ADDR_M6_PWM: M6_Dir = I2CDataIn >> 7 & 0x01;//take top bit if (((Status2 & M6_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) { M6_PWM = 0; } else { if (I2CDataIn & 0x80) { mPORTBClearBits(BIT_11); M6_PWM = (((int) ((~I2CDataIn) + 1)) << 4); } else { mPORTBSetBits(BIT_11); M6_PWM = ((int) I2CDataIn) << 4; } } SetDCOC2PWM(M6_PWM); break; case ADDR_M7_PWM: M7_Dir = I2CDataIn >> 7 & 0x01;//take top bit if (((Status2 & M7_CUR) && (config1 & CUR_SENSE_EN)) || (config1 & MOTOR_HALT)) { M7_PWM = 0; } else { if (I2CDataIn & 0x80) { mPORTBClearBits(BIT_7); M7_PWM = (((int) ((~I2CDataIn) + 1)) << 4); } else { mPORTBSetBits(BIT_7); M7_PWM = ((int) I2CDataIn) << 4; } } SetDCOC1PWM(M7_PWM); break; case ADDR_Config1: config1 = I2CDataIn; if (config1 & MOTOR_HALT) { SetDCOC1PWM(0); SetDCOC2PWM(0); SetDCOC3PWM(0); SetDCOC4PWM(0); SetDCOC5PWM(0); } else { SetDCOC1PWM(M7_PWM); SetDCOC2PWM(M6_PWM); SetDCOC3PWM(M5_PWM); SetDCOC4PWM(M3_PWM); SetDCOC5PWM(M4_PWM); } break; default: break; } I2Cstate = 0; } } else if ((I2C1STATbits.R_W == 1) && (I2C1STATbits.D_A == 0)) {
static PT_THREAD(protothread_CSense(struct pt *pt)) { PT_BEGIN(pt); while (1) { if (config1 & CUR_SENSE_EN) { if(Status2 & M7_CUR){ if(M7_LastDir != M7_Dir)//if direction has changed, lower CS flag, which enables PWM writing Status2 &= !M7_CUR; } if(Status2 & M6_CUR){ if(M6_LastDir != M6_Dir) Status2 &= !M6_CUR; } if(Status2 & M6_CUR){ if(M6_LastDir != M6_Dir) Status2 &= !M6_CUR; } if(Status2 & M5_CUR){ if(M5_LastDir != M5_Dir) Status2 &= !M5_CUR; } if(Status2 & M4_CUR){ if(M4_LastDir != M4_Dir) Status2 &= !M4_CUR; } if(Status2 & M3_CUR){ if(M3_LastDir != M3_Dir) Status2 &= !M3_CUR; } if (mPORTBReadBits(BIT_3)) {//CS triggered! M7_PWM = 0; SetDCOC1PWM(M7_PWM);//Halt motor Status2 |= M7_CUR;//set CS flag M7_LastDir = M7_Dir;//save direction } if (mPORTBReadBits(BIT_1)) { M6_PWM = 0; SetDCOC2PWM(M6_PWM); Status2 |= M6_CUR; M6_LastDir = M6_Dir; } if (mPORTAReadBits(BIT_3)) { M5_PWM = 0; SetDCOC3PWM(M5_PWM); Status2 |= M5_CUR; M5_LastDir = M5_Dir; } if (mPORTBReadBits(BIT_4)) { M4_PWM = 0; SetDCOC5PWM(M4_PWM); Status2 |= M4_CUR; M4_LastDir = M4_Dir; } if (mPORTAReadBits(BIT_4)) { M3_PWM = 0; SetDCOC4PWM(M3_PWM); Status2 |= M3_CUR; M3_LastDir = M3_Dir; } } PT_YIELD_TIME_msec(10); } PT_END(pt); }
//********************************************************************* //* PWM output only works on the pins with hardware support. //* These are defined in the appropriate pins_*.c file. //* For the rest of the pins, we default to digital output. //********************************************************************* void analogWrite(uint8_t pin, int val) { // We need to make sure the PWM output is enabled for those pins // that support it, as we turn it off when digitally reading or // writing with them. Also, make sure the pin is in output mode // for consistenty with Wiring, which doesn't require a pinMode // call for the analog output pins. pinMode(pin, OUTPUT); if (val == 0) { digitalWrite(pin, LOW); } else if (val == 255) { digitalWrite(pin, HIGH); } else { switch(digitalPinToTimer(pin)) { #ifdef _OCMP1 case TIMER_OC1: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 ); //Set duty cycle on fly SetDCOC1PWM((PWM_TIMER_PERIOD*val)/256); break; #endif #ifdef _OCMP2 case TIMER_OC2: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 ); //Set duty cycle on fly SetDCOC2PWM((PWM_TIMER_PERIOD*val)/256); break; #endif #ifdef _OCMP3 case TIMER_OC3: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 ); //Set duty cycle on fly SetDCOC3PWM((PWM_TIMER_PERIOD*val)/256); break; #endif #ifdef _OCMP4 case TIMER_OC4: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC4( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 ); //Set duty cycle on fly SetDCOC4PWM((PWM_TIMER_PERIOD*val)/256); break; #endif #ifdef _OCMP5 case TIMER_OC5: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC5( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, (PWM_TIMER_PERIOD*val)/256, (PWM_TIMER_PERIOD*val)/256 ); //Set duty cycle on fly SetDCOC5PWM((PWM_TIMER_PERIOD*val)/256); break; #endif #if 0 //* this is the original code, I want to keep it around for refernce for a bit longer #ifdef _OCMP1 case TIMER_OC1: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC1( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) ); // SetDCOC1PWM((PWM_TIMER_PERIOD * val) / 256); break; #endif #ifdef _OCMP2 case TIMER_OC2: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC2( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) ); // SetDCOC2PWM((PWM_TIMER_PERIOD * val) / 256); break; #endif #ifdef _OCMP3 case TIMER_OC3: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC3( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) ); // SetDCOC3PWM((PWM_TIMER_PERIOD * val) / 256); break; #endif #ifdef _OCMP4 case TIMER_OC4: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC4( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) ); // SetDCOC4PWM((PWM_TIMER_PERIOD * val) / 256); break; #endif #ifdef _OCMP5 case TIMER_OC5: //* Open Timer2 with Period register value OpenTimer2(T2_ON | T2_PS_1_256, PWM_TIMER_PERIOD); OpenOC5( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH, 256, (256 - val) ); // SetDCOC5PWM((PWM_TIMER_PERIOD * val) / 256); break; #endif #endif case NOT_ON_TIMER: default: if (val < 128) { digitalWrite(pin, LOW); } else { digitalWrite(pin, HIGH); } } } }