void PmodHB5getQEncRPM(HBRIDGE *hBridge,uint32_t pollsPerSec)
{
//read and calculate the current quadratue encoding
uint8_t newQuadPos = PORTReadBits(hBridge->sensorAport,hBridge->sensorAportBit) ^ PORTReadBits(hBridge->sensorBport,hBridge->sensorBportBit) ;
pollsPerSec /= 5; //calculate the RPM every 200ms
hBridge->pollCount++; //increase the number of polls total
//the position has changed
if(newQuadPos != hBridge->quadPos)
{
hBridge->quadPos = newQuadPos;
hBridge->pulseCount++;
}
//number of polls reached for RPM calculation
if(hBridge->pollCount == pollsPerSec)
{
hBridge->prevRpm = hBridge->rpm;
hBridge->rpm = (hBridge->pulseCount/4) * 5;
hBridge->pulseCount = 0;
hBridge->pollCount = 0;
}
}
inline CPLD_SPI_CONFIGURATION GetSPIChannel(void)
{
if(PORTReadBits(IOPORT_G, BIT_12))
return CPLD_SPI3A;
return CPLD_SPI2A;
}
void MyWIFI_Debug(void) {
char theStr[128];
sprintf(theStr, "WF_INT_IO=%x WF_INT_IF=%x WF_INT_IE=%x WF_HIB_IO=%x CS=%x \n", WF_INT_IO, WF_INT_IF, WF_INT_IE, WF_HIBERNATE_IO,
PORTReadBits(IOPORT_E, CS_FPGA | CS_WIFI | CS_MIWI | CS_IO | CS_FLASH));
MyConsole_SendMsg(theStr);
}
/********************************************************************
Funciton: void CUPLDConfigure(CPLD_CONFIGURATION configuration
********************************************************************/
CPLD_GFX_CONFIGURATION CPLDGetGraphicsConfiguration(void)
{
if(PORTReadBits(IOPORT_G, BIT_14))
return CPLD_GFX_CONFIG_16BIT;
return CPLD_GFX_CONFIG_8BIT;
}
void __ISR(_CHANGE_NOTICE_VECTOR, ipl6) ChangeNotice_Handler(void)
{
// Step #1 - always clear the mismatch condition first
int dummy = PORTReadBits(IOPORT_D, BIT_5);;
// Step #2 - then clear the interrupt flag
mCNClearIntFlag();
PORTF ^= BIT_0;
}
/********************************************************************
Funciton: void JoyStickTask(void)
********************************************************************/
void JoyStickTask(void)
{
joystickCntrl.value = 0;
if(!PORTReadBits(IOPORT_B, BIT_0))
joystickCntrl.left = 1;
if(!PORTReadBits(IOPORT_B, BIT_1))
joystickCntrl.up = 1;
if(!PORTReadBits(IOPORT_B, BIT_3))
joystickCntrl.down = 1;
if(!PORTReadBits(IOPORT_B, BIT_4))
joystickCntrl.right = 1;
if(!PORTReadBits(IOPORT_B, BIT_15))
joystickCntrl.fire = 1;
}
CPU_INT08U START_State (void)
{
#ifdef _TARGET_440H
CLIC_state(SW2);
#else
if(PORTReadBits(IO_START)!=0)
return 1;
else
return 0;
#endif
}
CPU_INT08U COLOR_Read (void)
{
#ifdef _TARGET_440H
CLIC_state(SW1);
#else
if(PORTReadBits(IO_COLOR)!=0)
return 1;
else
return 0;
#endif
}
/*
* Reads the state of the specified sensor pin for the specified hbridge.
*/
static uint8_t read_sensor_state(uint8_t hbridge_id,
enum hbridge_sensors sensor)
{
uint8_t retval = 0;
enum motor_list motor = (enum motor_list)hbridge_id;
if (motor < NUM_MOTORS)
{
retval = PORTReadBits(Motors[motor].sensor_port[sensor],
Motors[motor].sensor_bit_pos[sensor]);
}
return retval;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// STEP 9. configure the CN interrupt handler
void __ISR(_CHANGE_NOTICE_VECTOR, ipl2) ChangeNotice_Handler(void)
{
unsigned int temp;
// clear the mismatch condition
temp = PORTReadBits(IOPORT_D, BIT_6);
// .. things to do .. toggle the led
mPORTDToggleBits(BIT_1);
mPORTEToggleBits(BIT_2);
// clear the interrupt flag
mCNClearIntFlag();
}
/********************************************************************
Funciton: void CPLDGetSPIConfiguration(CPLD_CONFIGURATION configuration
********************************************************************/
CPLD_DEVICE_CONFIGURATION CPLDGetDeviceConfiguration(void)
{
uint8_t device;
device = (uint8_t)PORTReadBits(IOPORT_A, (BIT_7 | BIT_6)) & (BIT_7 | BIT_6);
device >>= 6;
switch(device)
{
case 0:
return CPLD_DEVICE_SPI_FLASH;
case 1:
return CPLD_DEVICE_WiFi;
case 2:
return CPLD_DEVICE_ZIGBEE;
}
return -1;
}
/** Soft SPI receive */
uint8_t spiRec(void) {
uint8_t data = 0;
// output pin high - like sending 0XFF
PORTSetBits(prtSDO, bnSDO);
for (uint8_t i = 0; i < 8; i++) {
PORTSetBits(prtSCK, bnSCK);
data <<= 1;
// adjust so SCK is nice
asm("nop");
asm("nop");
if (PORTReadBits(prtSDI,bnSDI)) data |= 1;
PORTClearBits(prtSCK, bnSCK);
}
return data;
}
void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl4) IC4Handler(void)
{
// clear the interrupt flag
//mIC5ClearIntFlag();
INTClearFlag(INT_IC4);
//mPORTDSetBits(BIT_2); //DEBUGGING
D11 = PORTReadBits(IOPORT_D, BIT_11);
if(D11>0 && timesReadIC4==0){
//mPORTDSetBits(BIT_0);
timeRight=delayRight;
timesReadIC4++;
}
if(D11==0 && timesReadIC4==1){
//mPORTDSetBits(BIT_1);
timeRight= delayRight - timeRight;
timesReadIC4=0;
rightDistance= timeRight * 10.0 / 58.0; //counterTimer2*periodTimer2 (10us)/58 = cm;
delayRight=0;
}
}
void __ISR(_INPUT_CAPTURE_2_VECTOR, ipl4) IC2Handler(void)
{
// clear the interrupt flag
//mIC5ClearIntFlag();
INTClearFlag(INT_IC2);
//mPORTDSetBits(BIT_2); //DEBUGGING
D9 = PORTReadBits(IOPORT_D, BIT_9);
if(D9>0 && timesReadIC2==0){
//mPORTDSetBits(BIT_0);
timeBack=delayBack;
timesReadIC2++;
}
if(D9==0 && timesReadIC2==1){
//mPORTDSetBits(BIT_1);
timeBack= delayBack - timeBack;
timesReadIC2=0;
backDistance= timeBack * 10.0 / 58.0; //counterTimer2*periodTimer2 (10us)/58 = cm;
delayBack=0;
}
}
void __ISR(_INPUT_CAPTURE_3_VECTOR, ipl4) IC3Handler(void)
{
// clear the interrupt flag
//mIC5ClearIntFlag();
INTClearFlag(INT_IC3);
//mPORTDSetBits(BIT_2); //DEBUGGING
D10 = PORTReadBits(IOPORT_D, BIT_10);
if(D10>0 && timesReadIC3==0){
//mPORTDSetBits(BIT_0);
timeLeft=delayLeft;
timesReadIC3++;
}
if(D10==0 && timesReadIC3==1){
//mPORTDSetBits(BIT_1);
timeLeft= delayLeft - timeLeft;
timesReadIC3=0;
leftDistance= timeLeft * 10.0 / 58.0; //counterTimer2*periodTimer2 (10us)/58 = cm;
delayLeft=0;
}
}
/******************** ULTRASONICS ***********************/
void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl4) IC1Handler(void)
{
// clear the interrupt flag
//mIC5ClearIntFlag();
INTClearFlag(INT_IC1);
//mPORTDSetBits(BIT_2); //DEBUGGING
D8 = PORTReadBits(IOPORT_D, BIT_8);
if(D8>0 && timesReadIC1==0){
//mPORTDSetBits(BIT_0);
timeFront=delayFront;
timesReadIC1++;
}
if(D8==0 && timesReadIC1==1){
//mPORTDSetBits(BIT_1);
timeFront= delayFront - timeFront;
timesReadIC1=0;
frontDistance= timeFront * 10.0 / 58.0; //counterTimer2*periodTimer2 (10us)/58 = cm;
delayFront=0;
}
}
CPU_INT08U CLIC_state (CPU_INT08U clic)
{
CPU_INT08U state = 0;
switch (clic) {
#ifdef _TARGET_440H
case SW1:
if(PORTReadBits(IOPORT_B, BIT_3)==0) state = 1;
break;
case SW2:
if(PORTReadBits(IOPORT_B, BIT_2)==0) state = 1;
break;
case SW3:
if(PORTReadBits(IOPORT_B, BIT_4)==0) state = 1;
break;
#else
case CLIC_1:
if(PORTReadBits(IO_CLIC_1)==0) state = 1;
break;
case CLIC_2:
if(PORTReadBits(IO_CLIC_2)==0) state = 1;
break;
case CLIC_3:
if(PORTReadBits(IO_CLIC_3)==0) state = 1;
break;
case CLIC_4:
if(PORTReadBits(IO_CLIC_4)==0) state = 1;
break;
#endif
default:
break;
}
return state;
}
void __ISR(_CHANGE_NOTICE_VECTOR, ipl2) ChangeNotice_Handler(void)
{
if(InTheMiddleOfSomething)
return;
BYTE bVal;
bVal = PORTReadBits(IOPORT_B, BIT_0 | BIT_3);
Led2;
/******************A front wall is detected******************/
//
if(!FrontSensor && LeftSensor)
{
InTheMiddleOfSomething = fTrue;
Led1;
Led2Clr;
//Reverse the motor
SetRightSpeed(dtcMtrStopped);
RightReverse; //SetRightDir(prtMtrRightDirClr); //Reverse
SetRightSpeed(dtcMtrMedium);
//mCNClearIntFlag();
//Wait until the turn is executed
Wait_ms(TURN90);
//Continue going straight
SetRightSpeed(dtcMtrStopped);
RightForward; //SetRightDir(prtMtrRightDirSet); //Forward
SetRightSpeed(dtcMtrMedium);
Led1Clr;
InTheMiddleOfSomething = fFalse;
mCNClearIntFlag();
return;
}
if(LeftSensor)
{
InTheMiddleOfSomething = fTrue;
Led4;
Led2Clr;
//Run for a few moments to get around the corner
Wait_ms(TURN90/2);
//Stop it until the turn is completed
SetLeftSpeed(dtcMtrStopped);
Wait_ms(TURN90);
//Continue going straight and wait to mare sure it can detect the new wall
SetLeftSpeed(dtcMtrMedium);
Wait_ms(TURN90/4);
Led4Clr;
InTheMiddleOfSomething = fFalse;
}
/*
if(LeftSensor && FrontSensor)
{
SetLeftSpeed(dtcMtrStopped);
SetRightSpeed(dtcMtrStopped);
}
else
{
if(LeftSensor) //Far left sensor -> hard left
{
SetLeftSpeed(dtcMtrMedium);
}
if(FrontSensor) //Far right sensor -> hard right
{
SetRightSpeed(dtcMtrMedium);
}
}
*/
mCNClearIntFlag();
}
unsigned getButtonState(unsigned int button) {
unsigned int buttonState = 0;
buttonState = PORTReadBits(IOPORT_G, button);
return buttonState;
}
/** A subclass may call EITHER read() OR read_analog(). */
virtual reg_t read_digital() {return PORTReadBits(ltr(), num());} // returns 0 or any non-0