/*
* 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;
}
}
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();
}*/
}
//Set the motor output, values from -1 to 1
void motorSet(double left, double right, char mode){
int val;
if( mode == MOTOR_MODE_BRAKE )
{
MOTOR_BRAKE_LEFT = 1;
MOTOR_BRAKE_RIGHT = 1;
MOTOR_DIR_LEFT = 1;
MOTOR_DIR_RIGHT = 1;
}
else{
if( left < 0.0 )
{
MOTOR_DIR_LEFT = 1;
MOTOR_BRAKE_LEFT = 0;
}
else
{
MOTOR_DIR_LEFT = 0;
MOTOR_BRAKE_LEFT = 1;
}
if( right < 0.0 )
{
MOTOR_DIR_RIGHT = 1;
MOTOR_BRAKE_RIGHT = 0;
}
else
{
MOTOR_DIR_RIGHT = 0;
MOTOR_BRAKE_RIGHT = 1;
}
val = fabsf(left) * MOTOR_PWM_PERIOD;
SetDCOC1PWM(val);
val = fabsf(right) * MOTOR_PWM_PERIOD;
SetDCOC2PWM(val);
}
}
err_t spp_recv(void *arg, struct rfcomm_pcb *pcb, struct pbuf *p, err_t err)
{
struct pbuf *q = NULL;
char cm1[3];
char cm2[3];
u8_t m;
static int lm1;
static int lm2;
int m1;
int m2;
LWIP_DEBUGF(BT_SPP_DEBUG, ("spp_recv: p->len == %d p->tot_len == %d\n", p->len, p->tot_len));
q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
if(p->tot_len != 0)
{
u8_t *data = p->payload;
*((u8_t*)q->payload) = *((u8_t *)p->payload);
if(*data == 'm')
{
m = 0x10;
cm1[0] = *(data+2);
cm1[1] = *(data+3);
cm1[2] = 0x00;
m1 = atoi(cm1);
if(m1 == 0){
m |= 0x03;
}else if(*(data+1) == '+'){
m |= 0x01;
}else if(*(data+1) == '-'){
m |= 0x02;
}
lm1 = m1;
cm2[0] = *(data+5);
cm2[1] = *(data+6);
cm2[2] = 0x00;
m2 = atoi(cm2);
if(m2 == 0){
m |= 0x0c;
}else if(*(data+4) == '+'){
m |= 0x04;
}else if(*(data+4) == '-'){
m |= 0x08;
}
lm2 = m2;
LWIP_DEBUGF(BT_SPP_DEBUG, ("spp_recv: m1 %d m2 %d\n", m1, m2));
SetDCOC1PWM((0xffff/10)*m1,0);
SetDCOC2PWM((0xffff/10)*m2,0);
#if defined(__PIC24FJ64GB002__)
mPORTAWrite(m);
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(m);
#endif
}else{
if(*data == 'a')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x15);
#endif
}else if(*data == 's')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1a);
#endif
}else if(*data == 'd')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x19);
#endif
}else if(*data == 'f')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x1f);
#endif
DelayMs(3);
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x16);
#endif
}else if(*data == 'g')
{
#if defined(__PIC24FJ64GB002__)
#elif defined(__PIC24FJ256GB106__)
mPORTEWrite(0x10);
#endif
}else{
}
}
}
if(rfcomm_cl(pcb)) {
rfcomm_uih_credits(pcb, PBUF_POOL_SIZE - rfcomm_remote_credits(pcb), q);
} else {
rfcomm_uih(pcb, rfcomm_cn(pcb), q);
}
pbuf_free(q);
pbuf_free(p);
return ERR_OK;
}
///////////////////////////////////////////////////////////
// PWM FUNCTIONS
///////////////////////////////////////////////////////////
void PWM_M1_SetDC(INT16U Duty)
{
SetDCOC2PWM((TMR3_RELOAD * Duty)/65536);
}
void setpwmL(int b)
{
SetDCOC2PWM(b); // Write new duty cycle
}
// 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;
}
/********************************************************************
* Tache: void TaskControl(void *pvParameters)
*
* Overview: Tâche responsable des différents lois de commande.
* Asservissement de position pour la direction, asservissement de
* courant (couple) pour la propulsion et supervision du niveau de
* batterie. Une fois les traitement fais, elle est responsable du
* post des grandeurs intermédaire pour le debug à la tâche d'affichage
*
* Auteur Date Commentaire
*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* Théou Jean-Baptiste 7 avril 2011 Première version
* Descoubes hugo 16 mai 2011 vs 1.1
*******************************************************************/
void TaskControl(void *pvParameters){
/* asservissement de position */
short sDirMeasure; // Mesure de position (servomoteur). Unsigned integer sur 10 bits
short sDirConsigne; // Consigne de position (servomoteur). Unsigned integer sur 10 bits
short sDirCommand; // Commande de position (servomoteur). entier compris entre 0 et 100, rapport cyclique pour PWM
/* asservissement de courant */
float fPropMeasure; // Mesure image du courant absorbé par la MCC (propulsion).
short sPropConsigne; // Consigne de courant(propulsion). Unsigned integer sur 10 bits
short sPropCommand; // Commande de courant (propulsion). entier compris entre 0 et 100, rapport cyclique pour PWM
short sPowerMeasure; // Mesure du niveau de batterie. Unsigned integer sur 10 bits
char stateControl = PROPULSION_CONTROL;// machnie d'état pour la commande
struct_DebugPrint printData; // Données à afficher (debug)
struct_mesures measuresReceive; // Mesures reçues depuis l'ISR du timer 3
for( ;; ){
/* Récupération des grandeurs de mesures pour les lois de commande */
xQueueReceive( xQueueAcquiData, &measuresReceive, portMAX_DELAY); // fonction bloquante
/* Mise à jour mesures */
sDirMeasure = measuresReceive.sDirMeasure;
fPropMeasure = (float) measuresReceive.sCurrentMeasure / 35.0; // 35 = facteur d'échelle (capteur + conditionneur + ADC)
sPowerMeasure = measuresReceive.sBattMeasure;
/* Mise à jour mesures - Affichage par le réseau */
resultat = sDirMeasure; // Mise à jour var. globale réseau
resultat_courant = measuresReceive.sCurrentMeasure; // Mise à jour var. globale réseau
/* Mise à jour consignes */
/* Protection Vitesse*/
xSemaphoreTake(xSemaphoreVitesse,portMAX_DELAY);
{
sPropConsigne = Vitesse; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreVitesse);
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
sDirConsigne = ConsDir; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreConsDir);
/* Machine d'état - tâche contrôle/commande/supervision */
switch(stateControl){
case PROPULSION_CONTROL :
/* Protection Consigne courant */
if(sPropConsigne > 100){
sPropConsigne = 100;
}
else if(sPropConsigne < 0){
sPropConsigne = 0;
}
/* Asservissement de courant (couple). Régulation PI (modèle non-linéaire) */
/* Calcul fais avec des flottant - temps de traitement long */
sPropCommand = propulsionCommand (sPropConsigne , fPropMeasure);
case PROPULSION_PWM :
if((fPropMeasure < 0.0) || (fPropMeasure > MAX_CURRENT) ){
sPropCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM propulsion */
/* Protection au niveau des erreurs de calculs */
if(sPropCommand < 10)
{
sPropCommand = 0;
}
SetDCOC4PWM(ReadPeriod3() * sPropCommand / 100 );
case DIRECTION_CONTROL :
/* Protection Consigne direction */
if(sDirConsigne > HAUT_BUTE){
sDirConsigne = HAUT_BUTE;
}
else if(sDirConsigne < BAS_BUTE){
sDirConsigne = BAS_BUTE;
}
/* Limitation sur la var globale ConsDir fait localement sur le MCU ... pour le moment ! */
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
ConsDir = sDirConsigne;
}
xSemaphoreGive(xSemaphoreConsDir);
/* Asservissement de position. Régulation proporionnelle (modèle grandement non-linéaire) */
sDirCommand = directionCommand(sDirConsigne, sDirMeasure);
case DIRECTION_PWM :
/* Vérification butées direction */
if( sDirMeasure < HAUT_BUTE && sDirMeasure > BAS_BUTE){
if(init_ok == 0){
/* Attendre initialisation utilisateur via réseau */
sDirCommand = 0;
}
else{
/* valeur absolue et sens de rotation - PWM comprise entre 0 et 100 */
if ( sDirCommand < 0 ){
/* Sens de rotation pour le driver de bras de pont */
PORTSetBits(BROCHE_DIR_DIRECTION);
sDirCommand = -sDirCommand;
}
else{
/* Sens de rotation pour le driver de bras de pont */
PORTClearBits(BROCHE_DIR_DIRECTION);
}
/* Protection et limitation Commande */
if(sDirCommand < 20){
sDirCommand = 0; // Pas de commande si dans dead zone
}
else if(sDirCommand < 30){
sDirCommand = 40; // compensation dead zone
}
else if (sDirCommand > 100){
sDirCommand = 100; // limitation PWM
}
}
}
else{
sDirCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM direction */
SetDCOC2PWM(ReadPeriod3() * sDirCommand / 100 );
case POWER_SUPERVIOR :
break;
default:
break;
}
/* Post les valeurs à afficher pour le debug vers la tâche d'affichage */
printData.commandeDir = sDirCommand;
printData.mesureDir = sDirMeasure;
printData.consigneDir = sDirConsigne;
printData.consigneMove = (float) sPropConsigne;
printData.mesureMove = fPropMeasure;
printData.commandeMove = sPropCommand;
printData.mesurePower = sPowerMeasure;
xQueueSend(xQueueDebugPrint, &printData, 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
