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;
}
}
/*
* 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;
}
}
}
int32_t main(void)
{
DDPCONbits.JTAGEN = 0; // Disable the JTAG programming port
/*
SYS_CFG_WAIT_STATES (configures flash wait states from system clock)
SYS_CFG_PB_BUS (configures the PB bus from the system clock)
SYS_CFG_PCACHE (configures the pCache if used)
SYS_CFG_ALL (configures the flash wait states, PB bus, and pCache)*/
/* TODO Add user clock/system configuration code if appropriate. */
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
/* TODO <INSERT USER APPLICATION CODE HERE> */
// initialise the IO pins and PWM outputs
TRISD = 0;
pickerBusTRIS = 0x00;
pickerBus = 0x00;
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // vibration motor
OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // head led
OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // base led
OpenTimer2( T2_ON | T2_PS_1_4 | T2_SOURCE_INT, 1024);
SetDCOC1PWM(0);
SetDCOC2PWM(0);
SetDCOC3PWM(0);
setVac1off;
setVac2off;
USBOutHandle = 0;
USBInHandle = 0;
USBDeviceInit(); //usb_device.c. Initializes USB module SFRs and firmware
USBDeviceAttach();
init_component_picker();
/*while(1)
{
LATBbits.LATB0 = feederXHome;
LATBbits.LATB1 = feederZHome;
//ProcessIO();
}*/
}
void setupPWMForIR()
{
// init OC3 module
OpenOC3(OC_OFF | OC_TIMER3_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
// init Timer3 mode and period (PR2) 40 kHz freq: 1 / 40 kHz = (X + 1) / 80MHz * 1
//80 000 / 20 = X + 1 = 3999
//80 000 / 38 = X + 1 = 2104
//80 000 / 36 = X + 1 = 2221
//80 000 / 40 = X + 1 = 2001
OpenTimer3(T3_ON | T3_PS_1_1 | T3_SOURCE_INT, MAX_DUTY);
SetDCOC3PWM(PR3 / 2);
}
// main() ---------------------------------------------------------------------
//
int main(void)
{
int pbClk; // Peripheral bus clock
// Configure the device for maximum performance, but do not change
// the PBDIV clock divisor. Given the options, this function will
// change the program Flash wait states, RAM wait state and enable
// prefetch cache, but will not change the PBDIV. The PBDIV value
// is already set via the pragma FPBDIV option above.
pbClk = SYSTEMConfig(CPU_HZ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
// The pic32 has 2 programming i/f's: ICD/ICSP and JTAG. The
// starter kit uses JTAG, whose pins are muxed w/ RA0,1,4 & 5.
// If we wanted to disable JTAG to use those pins, we'd need:
#if defined M4557_DUINOMITE
DDPCONbits.JTAGEN = 0; // Disable the JTAG port.
#endif
// Pins that share ANx functions (analog inputs) will default to
// analog mode (AD1PCFG = 0x0000) on reset. To enable digital I/O
// Set all ones in the ADC Port Config register. (I think it's always
// PORTB that is shared with the ADC inputs). PORT B is NOT 5v tolerant!
// Also, RB6 & 7 are the ICD/ICSP lines.
AD1PCFG = 0xffff; // Port B as digital i/o.
//Initialize the DB_UTILS IO channel
DBINIT();
#if defined ST7565_M4557_PROTOTYPE_STARTERKIT
// Enable pullup resistors for Starter Kit's 3 switches.
//CNPUE = (CN15_PULLUP_ENABLE | CN16_PULLUP_ENABLE | CN19_PULLUP_ENABLE);
mCNOpen(CN_ON, CN15_ENABLE | CN16_ENABLE,
CN15_PULLUP_ENABLE | CN16_PULLUP_ENABLE | CN19_PULLUP_ENABLE);
// Read the port to clear any mismatch on change notice pins
dummy = mPORTDRead();
// Clear change notice interrupt flag
ConfigIntCN(CHANGE_INT_ON | CHANGE_INT_PRI_2);
// Ok now to enable multi-vector interrupts
INTEnableSystemMultiVectoredInt();
// Display the introduction
DBPUTS("SW1:Set contrast; SW2:Halt display\n");
//DBPRINTF("DBPRINTF: The build date and time is (" __DATE__ "," __TIME__ ")\n");
// Init ports for ST7565 parallel prototype setup (using pic32 starter kit
// and i/o expansion board): TODO: This belongs somewhere else!!
//
// PIC32 NHD Display
// RB10 /CS1, /RES, A0, /WR, /RD
// RE0..7 D0..7
LATBSET = 0x7C00; // Set the control bits high (inactive)
TRISBCLR = 0x7C00; // Set the control bits as outputs.
// Init ESI touch-screen controller's (TSC2046) CS line as output
LATFSET = BIT_5; // Set CSn inactive...
TRISFCLR = BIT_5; // and set as output
// Data is on port E. The LCD controller (ST7565) uses its parallel
// mode on port E, RE0..7. The touch screen controller shares port
// shares a serial interface on some of these bits.
TRISECLR = 0x00ff; // Set the cmd/data bits as outputs.
#elif defined M4557_DUINOMITE
// Init ports for the M4557.
//
// Control bits are on port B:
//
// bit M4557 function
// ---- --------------
// 3 /CS1 - LCD Chip sel (ST7565 controller)
// 4 /RES - Reset
// 6 A0 -
// 7 /WR - Write
// 9 /RD - Read
// 10 /TPCS - Touch panel Chip Sel (TSC2046)
//
// Set the control bits low, and enable as outputs.
LATBSET = (BIT_3|BIT_4|BIT_6|BIT_7|BIT_9|BIT_10);
TRISBCLR = (BIT_3|BIT_4|BIT_6|BIT_7|BIT_9|BIT_10);
// Data is on port E. The LCD controller (ST7565) uses its parallel
// mode on port E, RE0..7. The touch screen controller shares port
// shares a serial interface on some of these bits.
TRISECLR = 0x00ff; // Set the cmd/data bits as outputs.
#else
#error Need product defined
#endif
Nop();
lcdInit(5,35); // Init lcd controller
Nop();
// Output Compare (PWM) pins
//
// OCn 64pin 100pin/port SKII-J11- Duinomite
// --- ----- ----------- -------------- ----------
// OC1 46 72/RD0 19 (LED1,Red) SOUND
// OC2 49 76/RD1 20 (LED2,Yel) D13,SD_CLK
// OC3 50 77/RD2 17 (LED3,Grn) D12,SD_MISO
// OC4 51 78/RD3 18 D11,SD_MOSI
// OC5 52 81 15 vga_hsync
//
// Init Timer 2 for use by the OC (PWM) module(s).
// This will set the PWM frequency, f = pbClk/reloadValue.
// Examples (pcClk = 40MHz):
// f = pbClk/100000 = 400Hz
// f = pbClk/10000 = 4kHz
// f = pbClk/2500 = 20kHz
//
//OpenTimer2(T2_ON | T2_32BIT_MODE_ON, 100000); // f = pbClk/100000 = 400Hz
//OpenTimer2(T2_ON | T2_32BIT_MODE_ON, 10000); // f = pbClk/10000 = 4kHz
OpenTimer2(T2_ON | T2_32BIT_MODE_ON, 2500); // f = pbClk/2500 = 16kHz
// I tried the uChip example using "OpenOC2", and as is ofter the
// case, I can't get it to work, the documentation is lacking, and
// it's easier to just read the datasheet and program the bloody
// OCxCON register directly.
//OpenOC2( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH , 40000, 30000 );
SetDCOC2PWM(0);
OC2CON = 0x8026; // OC on; 32bit-mode; PWM mode.
SetDCOC3PWM(0);
OC3CON = 0x8026; // OC on; 32bit-mode; PWM mode.
/*
int testOnly = 1;
uint32_t loadVal = 0;
int i;
while(testOnly)
{
// Try the dimmer's 16 levels (0..15)
for(i=0; i<16; i++)
{
loadVal = pwmTable1[i];
SetDCOC2PWM(loadVal);
delay_ms(300);
}
}
CloseOC2();
*/
// Do ESI M4557 demo application (dimmer-control type demo w/ touchscreen)
esi_M4557();
return 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);
}
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)) {
//*********************************************************************
//* 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);
}
}
}
}
// Update motor state.
// MOTOR_STATE dir: MOTOR_STOP, MOTOR_FORWARD, MOTOR_BACKWARD
// uint speed: 0 - 1000
void MotorState(BYTE motor, MOTOR_STATE dir, WORD speed)
{
//debug("M%d d%d s%d\r\n", motor, dir, speed);
if(dir == MOTOR_FORWARD)
{
if(motor == 1)
{
M1_FORWARD_IO = 1;
M1_BACKWARD_IO = 0;
}
else if(motor == 2)
{
M2_FORWARD_IO = 1;
M2_BACKWARD_IO = 0;
}
else if(motor == 3)
{
M3_FORWARD_IO = 1;
M3_BACKWARD_IO = 0;
}
}
else if(dir == MOTOR_BACKWARD)
{
if(motor == 1)
{
M1_FORWARD_IO = 0;
M1_BACKWARD_IO = 1;
}
else if(motor == 2)
{
M2_FORWARD_IO = 0;
M2_BACKWARD_IO = 1;
}
else if(motor == 3)
{
M3_FORWARD_IO = 0;
M3_BACKWARD_IO = 1;
//debug("b");
}
}
else
{
if(motor == 1)
{
M1_FORWARD_IO = 0;
M1_BACKWARD_IO = 0;
}
else if(motor == 2)
{
M2_FORWARD_IO = 0;
M2_BACKWARD_IO = 0;
}
else if(motor == 3)
{
M3_FORWARD_IO = 0;
M3_BACKWARD_IO = 0;
}
}
speed += 1000; // code originally setup to run on pwm pulses 1000-2000
if(speed >= 900 && speed <= 2100)
{
if(motor == 1)
SetDCOC1PWM((int)(speed * MOTOR_SPEED_MULT));
else if(motor == 2)
SetDCOC2PWM((int)(speed * MOTOR_SPEED_MULT));
else if(motor == 3)
SetDCOC3PWM((int)(speed * MOTOR_SPEED_MULT));
//else if(motor == 4)
// SetDCOC4PWM((int)(speed * MOTOR_SPEED_MULT));
}
}
/********************************************************************
* Function: void ProcessIO(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: This function is a place holder for other user
* routines. It is a mixture of both USB and
* non-USB tasks.
*
* Note: None
*******************************************************************/
void ProcessIO(void)
{
// User Application USB tasks
if((USBDeviceState < CONFIGURED_STATE)||(USBSuspendControl==1)) return;
//Check if we have received an OUT data packet from the host
if(!HIDRxHandleBusy(USBOutHandle))
{
//We just received a packet of data from the USB host.
//Check the first byte of the packet to see what command the host
//application software wants us to fulfill.
switch (ReceivedDataBuffer[0]) {
case 0x01: // System Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // System Commands
// Copy any waiting debug text to the send data buffer
ToSendDataBuffer[0] = 0xFF;
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
default: // Unknown command received
break;
}
break;
case 0x02: // Feeder Commands
switch (ReceivedDataBuffer[1]) {
case 0x02: // Feeder Status
if (FeederStatus() == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x03: // Go to feeder
if ((ReceivedDataBuffer[2] >= 0) && (ReceivedDataBuffer[2] <= 16)){
moveToPosition(ReceivedDataBuffer[2]);
}
break;
case 0x04: // Picker Up
PickerUp();
break;
case 0x05: // Zero Feeder
ZeroFeeder();
break;
case 0x06: // Picker Down
PickerDown();
break;
case 0x07: // Set Picker Port Output
pickerBusval = ((ReceivedDataBuffer[3] << 8) | ReceivedDataBuffer[2]);
pickerBus = pickerBusval;
break;
case 0x08: // Go to feeder
if ((ReceivedDataBuffer[2] > 0) && (ReceivedDataBuffer[2] <= 16)){
moveToPositionWithoutPick(ReceivedDataBuffer[2]);
}
break;
default: // Unknown command received
break;
}
break;
case 0x03: // Vacuum and Vibration Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // Vacuum 1 set
if (ReceivedDataBuffer[2] == 0x01){
setVac1on;
}
else{
setVac1off;
}
break;
case 0x02: // Vacuum 2 set
if (ReceivedDataBuffer[2] == 0x01){
setVac2on;
}
else{
setVac2off;
}
break;
case 0x03: // Vibration Motor set
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC1PWM(vibrationmotor_duty_cycle);
vibrationrunning = 1;
}
else{
SetDCOC1PWM(0);
vibrationrunning = 0;
}
break;
case 0x04: // Vacuum 1 status
if (vac1running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x05: // Vacuum 2 status
if (vac2running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x06: // Vibration Motor status
if (vibrationrunning == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x07: // Vibration Motor status
vibrationmotor_duty_cycle = ReceivedDataBuffer[2] * 4;
if (vibrationrunning == 1){
SetDCOC1PWM(vibrationmotor_duty_cycle);
}
break;
default: // Unknown command received
break;
}
break;
case 0x04: // LED Commands
switch (ReceivedDataBuffer[1]) {
case 0x01: // LED Base Camera on/off
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC2PWM(led1_duty_cycle);
led1running = 1;
}else{
SetDCOC2PWM(0);
led1running = 0;
}
break;
case 0x02: // LED Base Camera PWM set
led1_duty_cycle = ReceivedDataBuffer[2] * 4;
if (led1running == 1){
SetDCOC2PWM(led1_duty_cycle);
}
break;
case 0x03: // LED Head Camera on/off
if (ReceivedDataBuffer[2] == 0x01){
SetDCOC3PWM(led2_duty_cycle);
led2running = 1;
}else{
SetDCOC3PWM(0);
led2running = 0;
}
break;
case 0x04: // LED Head Camera PWM set
led2_duty_cycle = ReceivedDataBuffer[2] * 4;
if (led2running == 1){
SetDCOC3PWM(led2_duty_cycle);
}
break;
case 0x05: // LED Base Status
if (led1running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
case 0x06: // LED Head Status
if (led2running == 1){
ToSendDataBuffer[0] = 0x01;
}
else{
ToSendDataBuffer[0] = 0x00;
}
// Transmit the response to the host
if (!HIDTxHandleBusy(USBInHandle)) {
USBInHandle = HIDTxPacket(HID_EP, (BYTE*) & ToSendDataBuffer[0], 64);
}
break;
default: // Unknown command received
break;
}
break;
default: // Unknown command received
break;
}
//Re-arm the OUT endpoint, so we can receive the next OUT data packet
//that the host may try to send us.
USBOutHandle = HIDRxPacket(HID_EP, (BYTE*)&ReceivedDataBuffer, 64);
}
}//end ProcessIO
