void KeyPad::scanForKeyPresses()
{
bool aKeyIsPressed = false;
// Set each column to high and see what rows output high (complete circuit)
for(int columnIndex = 0; columnIndex < 3; columnIndex++)
{
// Turn on column pin
//PORTD |= (1 << (COLUMN_1 + columnIndex));
SetPin(D, COLUMN_1 + columnIndex, 1);
for(int rowIndex = 0; rowIndex < 4; rowIndex++)
{
// Get the state of the pin we actually need
pinState = PINC & (1 << (ROW_1 + rowIndex));
if(pinState >= 1)
{
aKeyIsPressed = true;
STATE.keyStates[rowIndex][columnIndex] = 1;
} else {
STATE.keyStates[rowIndex][columnIndex] = 0;
}
}
// Turn off column pin
//PORTD ^= (1 << (COLUMN_1 + columnIndex));
SetPin(D, COLUMN_1 + columnIndex, 0);
}
if(!aKeyIsPressed) SetPin(C, 4, 1); // Set the status light to HIGH if no key is pressed
else SetPin(C, 4, 0); // Set the status light to LOW if a key is pressed
}
void findEnd(void){
static int endFound = 0;
static int bitchin = 0;
SetPin(PIN_F1, 0);
SetPin(PIN_F3, 1);
if (wasLeftWall) {
SetMotor(leftMotor, .5);
SetMotor(rightMotor, .2);
}
else {
SetMotor(leftMotor, .2);
SetMotor(rightMotor, .5);
}
if (!linePresent()||mostDark()){
endFound+=1;
}else{
endFound = 0;
bitchin = 0; ///new addition
}
if(endFound == 50){
bitchin+=1;
endFound = 0;
}
if (bitchin==6){
stage = 3;
}
SetPin(PIN_F1, 1);
SetPin(PIN_F3, 0);
}
void TBBrake(void) {
SetPin(L1, 1);
SetPin(L2, 1);
SetPin(R1, 1);
SetPin(R2, 1);
}
void main(void) {
uint16 speed = param_speed;
uint8 port = param_port,
pin=param_pin;
uint8 ServoPos = 120;
uint8 dir=1;
systemInit();
usbInit();
InitServos();
EA=1; // Global interrupt enabled
SetPin(0, port, pin);
SetPin(1, 0, 1);
SetPin(2, 0, 3);
while (1) {
delayMs(speed);
usbComService();
if (dir==1) {
++ServoPos;
}
else {
--ServoPos;
}
if (ServoPos > 253 || ServoPos < 2) {
dir ^=1;
}
SetPos(0,ServoPos);
SetPos(1,128);
SetPos(2,64);
}
}
void TBCoast(void) {
SetPin(L1, 0);
SetPin(L2, 0);
SetPin(R1, 0);
SetPin(R2, 0);
}
void InitRGB( struct SRGBLEDControl *pData )
{
if ( !bInitialised )
{
}
SetPin( pData->PortR, pData->Options & _BV(RGB_SINK) );
SetPin( pData->PortG, pData->Options & _BV(RGB_SINK) );
SetPin( pData->PortB, pData->Options & _BV(RGB_SINK) );
bInitialised = true;
}
void Backward (void) {
uprintf("Command Excepted\r");
setDirL(-1);
setDirR(-1);
SetPin('B',0,0);
SetPin('B',1,1);
SetPin('B',2,0);
SetPin('B',3,1);
setCHA10(ExtractNum(8));
setCHB10(ExtractNum(8));
}
void SpinL (void) {
uprintf("Command Excepted\r");
setDirL(-1);
setDirR(1);
SetPin('B',0,0);
SetPin('B',1,1);
SetPin('B',2,1);
SetPin('B',3,0);
setCHA10(ExtractNum(5));
setCHB10(ExtractNum(5));
}
void Stop (void) {
uprintf("Command Excepted\r");
setDirL(0);
setDirR(0);
SetPin('B',0,0);
SetPin('B',1,0);
SetPin('B',2,0);
SetPin('B',3,0);
setCHA10(0);
setCHB10(0);
}
void Forward (void) {
uprintf("Command Excepted\r");
setDirL(1);
setDirR(1);
SetPin('B',0,1);
SetPin('B',1,0);
SetPin('B',2,1);
SetPin('B',3,0);
setCHA10(ExtractNum(7));
setCHB10(ExtractNum(7));
}
void TurnR (void) {
uprintf("Command Excepted\r");
setDirL(1);
setDirR(0);
SetPin('B',0,1);
SetPin('B',1,0);
SetPin('B',2,0);
SetPin('B',3,0);
setCHA10(ExtractNum(5));
setCHB10(ExtractNum(5));
}
/**
@author Reid Kersey <*****@*****.**> 678-736-0899
@brief This function changes the TX power level to be higher or lower
@details This function sets the devices CMD pin, and then sends the code
for setting the power level, and then it sets the power level. 100uS sleep
times are adhered to.
@param device This is a sturcture of settings for the wireless device
*/
void setPowerLevel(struct WirelessPins *device){
SetPin(device->cmd, device->cmd_row, 1);
usleep(100);
fprintf(wirelessFile, "%c", (char)0x3A);
if(powerLevel < 0) powerLevel = 0;
if(powerLevel > 4) powerLevel = 4;
fprintf(wirelessFile, "%c", (char)powerLevel);
usleep(100);
logMessage("TX Transmit power level changed", 0);
SetPin(device->cmd, device->cmd_row, 1);
}
void TBBackward(float speed_left, float speed_right) {
// set logic bits
SetPin(L1, 0);
SetPin(L2, 1);
SetPin(R1, 0);
SetPin(R2, 1);
// activate PWMs with speed
SetPWM(pwm_left, speed_left, 0);
SetPWM(pwm_right, speed_right, 0);
}
void grabBall(void)
{
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
//Open hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(1.0);
lowerArm();
//Close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(1.0);
//arm goes up
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArm();
}
bool ChannelPinMapper::TogglePin(int pinIdx, int chIdx)
{
bool on = GetPin(pinIdx, chIdx);
on = !on;
SetPin(pinIdx, chIdx, on);
return on;
}
void led_state(pyb_led_t led, int state) {
if (led < 1 || led > NUM_LEDS) {
return;
}
int pinapi_id = pyb_led_obj[led - 1].pinapi_id;
SetPin(pinapi_id, state);
//xil_printf("\nLED STATE: %d", state);
/*
if (state == 0) {
SetPin(LED1, state);
} else {
SetPin(LED1, state);
}
*/
/*
const pin_obj_t *led_pin = pyb_led_obj[led - 1].led_pin;
//printf("led_state(%d,%d)\n", led, state);
if (state == 0) {
// turn LED off
MICROPY_HW_LED_OFF(led_pin);
} else {
// turn LED on
MICROPY_HW_LED_ON(led_pin);
}
*/
}
int main(void) {
initIRSensor();
initMotor();
initGPIOLineSensor();
initServo();
SetServo(servo,0);
stage = 0;
//while(1){followLine();}
//while(1){followWall();}
//SetMotor(leftMotor, 1); SetMotor(rightMotor, 1);
while(true){
LineSensorReadArray(gls, line);
if (stage==0){ //start state
if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) {
followWall();
}else{
followLine();
}
}else if(stage==1){
//once 90degree turn passed
// SetMotor(leftMotor, 1);
// SetMotor(rightMotor, -1);
SetPin(PIN_F2, 1);
followWall();
if (wallPresent()) {followWall();}
else {
findLine();
}
SetPin(PIN_F2, 0);
}else if (stage==2){
//once line found again after walled section
SetPin(PIN_F1, 1);
if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)||
(mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5)
findEnd();
}else{
followLine();
};
SetPin(PIN_F1, 0);
}else{//end of course look for flag
findObject();
break;
}
}
}
void ChannelPinMapper::SetNChannels(int nCh)
{
int i;
for (i = m_nCh; i < nCh && i < m_nPins; ++i) {
SetPin(i, i, true);
}
m_nCh = nCh;
}
KeyPad::KeyPad()
{
// Initialize key states
for(int columnIndex = 0; columnIndex < 3; columnIndex++)
{
for(int rowIndex = 0; rowIndex < 4; rowIndex++)
{
previousKeyStates[rowIndex][columnIndex] = 0;
}
}
// Initialize pin states and pin I/O setting
DDRD |= 0b01110000; // 3 Column Pins as outputs on 4-6
DDRC |= 0b0010000; // 4 Row Pins as inputs on 0-3 and status pin on 4 as output
SetPin(D, 6, 0);
SetPin(D, 5, 0);
SetPin(D, 4, 0);
}
Motor32::Motor32( uint8_t pin)
{
if (PinAvailable(pin))
SetPin(pin);
}
bool PortParameters::SetBit(const char *pVal, const Bit &bit)
{
if (!lstrcmpi(pVal, "*"))
return TRUE;
if (!lstrcmpi("-", pVal)) {
if (maskExplicit & bit.bit) {
maskExplicit &= ~bit.bit;
maskChanged |= bit.bit;
}
return TRUE;
}
if (bit.type == Bit::FLAG) {
if (!SetFlag(pVal, bit.bit))
return FALSE;
}
else
if (bit.type == Bit::PIN) {
if (!SetPin(pVal, bit.bit))
return FALSE;
}
else
if (bit.type == Bit::PROBABILITY) {
if (!SetProbability(pVal, bit.bit))
return FALSE;
}
else
if (bit.type == Bit::UNSIGNED) {
if (!SetUnsigned(pVal, bit.bit))
return FALSE;
}
else
if (bit.type == Bit::OTHER) {
if (bit.bit == m_portName) {
if (!SetPortName(pVal))
return FALSE;
}
else
if (bit.bit == m_realPortName) {
if (!SetRealPortName(pVal))
return FALSE;
}
else {
return FALSE;
}
}
else {
return FALSE;
}
if (maskChanged & bit.bit)
maskExplicit |= bit.bit;
return TRUE;
}
void ChannelPinMapper::SetNPins(int nPins)
{
if (nPins<0) nPins=0;
else if (nPins>CHANNELPINMAPPER_MAXPINS) nPins=CHANNELPINMAPPER_MAXPINS;
int i;
for (i = m_nPins; i < nPins; ++i) {
ClearPin(i);
if (i < m_nCh) {
SetPin(i, i, true);
}
}
m_nPins = nPins;
}
// Initializes mem card
char MEM_init()
{
char val;
mem_bus_granted = 0;
// acquire the SPI bus and other necesary signals
val = MEM_acquireControl();
if(val)
return(val);
// reset to known idle state
// assert reset, wait and deassert reset
ClearPin(PORTC, MEM_RESET);
delay(1);
SetPin(PORTC, MEM_RESET);
// get memory status
// select memory
SPI_select(MEMCS);
// read status register
SPI_send(READ_STATUS_REGISTER);
// get return value
val = SPI_receive(0x00);
SPI_deselect();
if(((val >> 2) & 0x0f) == 0x0f) {
if(!(val & 0x01)) {
MEM.mem_size = 8650752;
MEM.page_size = 1056;
}
else {
blinkLED(10);
}
MEM.fbell_offset = MEM.page_size;
MEM.rbell_offset = MEM.mem_size / 2;
}
else
void dropBall(void)
{
//move arm up halfway
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArmPeak();
//open hand and wait for ball to drop
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(3.0);
//close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(2.0);
//arm goes down
raiseArm();
}
void blink(void) {
SetPin(PIN_F2, blink_on);
SetPin(PIN_F1, blink_on);
SetPin(PIN_D1, blink_on);
blink_on = !blink_on;
}
//Flashes blue and red LEDs
void blink(void) {
SetPin(PIN_F1, led_on);
SetPin(PIN_F3, !led_on);
led_on = !led_on;
}
//Turns on Red LED
void ledRed(void){
SetPin(PIN_F1,true);
SetPin(PIN_F2, false);
SetPin(PIN_F3, false);
}
//Turns on Blue LED
void ledBlue(void){
SetPin(PIN_F1,false);
SetPin(PIN_F2, true);
SetPin(PIN_F3, false);
}
void ledGreen(void){
SetPin(PIN_F1, false);
SetPin(PIN_F2, false);
SetPin(PIN_F3, true);
}
void GPIO_PORT::SetupGPIO_InPullUp (GPIO_PIN_N pin)
{
SetupGPIO (pin, GPIO_CNF_IPUPD, GPIO_MODE_INP);
SetPin (pin);
}
