int main(void) { SPI_MasterInit(); btInit(); interruptINT1_init(); sei(); btTransmit(0); //moveRobot(ACTIVATE_HIT); while(1) { BT_SensorValues(); getSensorValues(); setVariables(); dataValues[LIFE] = lifeCount; //Start of AI program that should keep the robot within the boundaries of the tape track moveRobot(LED | lifeCount); if(!lifeCount) { sensorControlDead(); if(TapeFlag >= 0x03 && counting == 0) { counting = 1; timerValue = TIMER_1A_SECOND; timer_init(); } else if(counting == 2) { while(1) { moveRobot(STOP); } } } else { if(hit == 0) { activateHitFlag = 0; hitFlag = 1; } if (hit == 1 && activateHitFlag) { moveRobot(IR_ON); moveRobot(ACTIVATE_HIT); } else if(hit == 1 && hitFlag == 1) { hitFlag = 0; lifeCount = lifeCount >> 1; timer0_init(); } else if (hit == 1 && !activateHitFlag) { moveRobot(IR_OFF); } if(!sprayPray) { idle(); } }
/*Thread to watch the sensors and send a message of events to the correct gate*/ void inputWatcher(){ char message[MAX_MESSAGE_LENGTH]; while(1){ newSensors = getSensorValues(); /*consider the read sensor values new*/ if(newSensors != oldSensors){ /*if the sensors have changed from the old one then continue*/ if( ( newSensors & IN_SENSORS ) != ( oldSensors & IN_SENSORS ) ){ /*if in gate sensors have changed:*/ sprintf(message, "IN:CHANGE FROM %d TO %d\n", oldSensors & IN_SENSORS, newSensors & IN_SENSORS); /*create message*/ if((msgQSend(inGateQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR){ /*send message to in gate*/ printf("msgQSend to gate in failed\n"); } } if ( (newSensors & OUT_SENSORS) != (oldSensors & OUT_SENSORS) ){ sprintf(message, "OUT:CHANGE FROM %d TO %d\n", oldSensors & OUT_SENSORS, newSensors & OUT_SENSORS); /*create message*/ if((msgQSend(outGateQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR){ /*send message to in gate*/ printf("msgQSend to gate out failed\n"); } } } taskDelay(6); /*one tenth of a second*/ oldSensors = newSensors; /*consider the read sensor values old*/ } }
void InputSample::update(float elapsedTime) { if (hasAccelerometer()) { Vector3 accelRaw, gyroRaw; getSensorValues(&accelRaw.x, &accelRaw.y, &accelRaw.z, &gyroRaw.x, &gyroRaw.y, &gyroRaw.z); // Adjust for landscape mode float temp = accelRaw.x; accelRaw.x = -accelRaw.y; accelRaw.y = temp; temp = gyroRaw.x; gyroRaw.x = -gyroRaw.y; gyroRaw.y = temp; // Respond to raw accelerometer inputs Vector3 position; _formNodeParent->getTranslation(&position); position.smooth(_formNodeRestPosition - accelRaw*0.04f, elapsedTime, 100); _formNodeParent->setTranslation(position); // Respond to raw gyroscope inputs Vector3 rotation; float angle = _formNodeParent->getRotation(&rotation); rotation *= angle; rotation.smooth(gyroRaw*(-0.18f), elapsedTime, 220); angle = rotation.length(); rotation.normalize(); _formNodeParent->setRotation(rotation, angle); } }
void simulator() { initiateProcess(); oldSensors = getSensorValues(); /*create both message queues*/ if ((gateInQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL) printf("msgQCreate failed for read to gate in queue\n"); if ((gateOutQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL) printf("msgQCreate failed for update to gate out queue\n"); if((taskSpawn("TASK 0",70,0x100,2000,(FUNCPTR)inputWatcher,0,0,0,0,0,0,0,0,0,0))== ERROR) { printf("Error spawning input watcher task.\n"); } /*TASK 1 = gate in*/ if((taskSpawn("GATE IN",70,0x100,2000,(FUNCPTR)gateIn,0,0,0,0,0,0,0,0,0,0))== ERROR) { printf("Error spawning gateIn task.\n"); } /*TASK 2 = gate out*/ if((taskSpawn("GATE OUT",70,0x100,2000,(FUNCPTR)gateOut,0,0,0,0,0,0,0,0,0,0))== ERROR) { printf("Error spawning gateOut task.\n"); } }
/*Set all ports to 0 and set actuators to both lights red*/ void initiateProcess() { sysOutByte(0x180,0x00); /* Zero out the four channels */ sysOutByte(0x181,0x00); sysOutByte(0x182,0x00); sysOutByte(0x183,0x00); sysOutByte(0x184,0x01); /* Enable Output */ /*set actuators to both lights red*/ iActuators = (IN_RED | OUT_RED); setActuatorValues(iActuators); /*the first sensor values, considered old, are obtained*/ oldSensors = getSensorValues(); }
void __ISR(_TIMER_5_VECTOR, IPL3AUTO) Timer5Handler(void) //~75hz { Orientation_Update(); INTClearFlag(INT_T5); // Be sure to clear the Timer1 interrupt status getSensorValues(); determineZeroYaw(); computePIDValues(); adjustOCValues(); read_voltage(); //LATFINV = BIT_1; }
int main(int argc, char *argv[]) { /* initialize Webots */ wb_robot_init(); reset(); /* main loop */ for (;;) { getSensorValues(distances); run(); /* perform a simulation step */ wb_robot_step(TIME_STEP); } return 0; }
/*TASK 0*/ void inputWatcher() { char message[MAX_MESSAGE_LENGTH]; while (1) { newSensors = getSensorValues(); /*read the sensors*/ if(newSensors != oldSensors){ /*create message and send it through the queue*/ sprintf(message, "CHANGE %d", newSensors); if((msgQSend(gateInQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR) printf("msgQSend to gate in failed\n"); /*both tasks wont work together - even with this message queue*/ if((msgQSend(gateOutQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR) printf("msgQSend to gate out failed\n"); } /* if(newSensors != oldSensors){ gateIn(); gateOut(); } */ taskDelay(6); /*Delay 6/60 -> 1/10 of a second*/ oldSensors = newSensors; /*update the sensors*/ } }
void stateMachine() { while (1) { if (zigbeeNetworkStatus == NWK_ONLINE) { if(moduleHasMessageWaiting()) //wait until SRDY goes low indicating a message has been received. { getMessage(); displayMessage(); } } switch (state) { case STATE_IDLE: { if (stateFlags & STATE_FLAG_SEND_INFO_MESSAGE) //if there is a pending info message to be sent { state = STATE_SEND_INFO_MESSAGE; //then send the message and clear the flag stateFlags &= ~STATE_FLAG_SEND_INFO_MESSAGE; } /* Other flags (for different messages or events) can be added here */ break; } case STATE_MODULE_STARTUP: { #define MODULE_START_DELAY_IF_FAIL_MS 5000 // Must be greater than MODULE_START_FAIL_LED_ONTIME moduleResult_t result; struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_END_DEVICE; /* Uncomment below to restrict the device to a specific PANID defaultConfiguration.panId = 0x1234; */ struct applicationConfiguration endDeviceConf; endDeviceConf.endPoint = 1; endDeviceConf.latencyRequested = LATENCY_NORMAL; endDeviceConf.profileId = 0xcc00; // the clock profile is 0xCC00 endDeviceConf.deviceId = 0x8866; endDeviceConf.deviceVersion = 0x01; endDeviceConf.numberOfBindingInputClusters = 4; // number of binding input cluster endDeviceConf.bindingInputClusters[0] = 0x0000; // basic cluster endDeviceConf.bindingInputClusters[1] = 0x0003; // identify cluster endDeviceConf.bindingInputClusters[2] = 0xfc01; // synchronise clock cluster endDeviceConf.bindingInputClusters[3] = 0xfc02; // send string message cluster endDeviceConf.numberOfBindingOutputClusters = 4; // number of binding output cluster endDeviceConf.bindingOutputClusters[0] = 0x0000; endDeviceConf.bindingOutputClusters[1] = 0x0003; endDeviceConf.bindingOutputClusters[2] = 0xfc01; endDeviceConf.bindingOutputClusters[3] = 0xfc02; struct applicationConfiguration ac; ac.endPoint = 1; ac.deviceVersion = 0x10; ac.profileId = 0xfafa; ac.latencyRequested = LATENCY_NORMAL; ac.numberOfBindingInputClusters = 2; ac.bindingInputClusters[0] = 0x0000; ac.bindingInputClusters[1] = 0x0900; ac.numberOfBindingOutputClusters = 2; ac.bindingOutputClusters[0] = 0x0000; ac.bindingOutputClusters[1] = 0x0900; /* Below is an example of how to restrict the device to only one channel: defaultConfiguration.channelMask = CHANNEL_MASK_17; printf("DEMO - USING CUSTOM CHANNEL 17\r\n"); */ //while ((result = startModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION)) != MODULE_SUCCESS) while ((result = startModule(&defaultConfiguration, &ac)) != MODULE_SUCCESS) { SET_NETWORK_FAILURE_LED_ON(); // Turn on the LED to show failure printf("FAILED. Error Code 0x%02X. Retrying...\r\n", result); delayMs(MODULE_START_DELAY_IF_FAIL_MS/2); SET_NETWORK_FAILURE_LED_OFF(); delayMs(MODULE_START_DELAY_IF_FAIL_MS/2); } INIT_BOOSTER_PACK_LEDS(); SET_NETWORK_LED_ON(); SET_NETWORK_FAILURE_LED_OFF(); printf("Success\r\n"); /* Module is now initialized so store MAC Address */ zbGetDeviceInfo(DIP_MAC_ADDRESS); memcpy(hdr.mac, zmBuf+SRSP_DIP_VALUE_FIELD, 8); #define MESSAGE_PERIOD_SECONDS 4 int16_t timerResult = initTimer(MESSAGE_PERIOD_SECONDS); if (timerResult != 0) { printf("timerResult Error %d, STOPPING\r\n", timerResult); while (1); } state = STATE_DISPLAY_NETWORK_INFORMATION; break; } case STATE_DISPLAY_NETWORK_INFORMATION: { printf("~ni~"); /* On network, display info about this network */ displayNetworkConfigurationParameters(); displayDeviceInformation(); /* Set module GPIOs as output and turn them off */ if ((sysGpio(GPIO_SET_DIRECTION, ALL_GPIO_PINS) != MODULE_SUCCESS) || (sysGpio(GPIO_CLEAR, ALL_GPIO_PINS) != MODULE_SUCCESS)) { printf("ERROR\r\n"); } state = STATE_SEND_INFO_MESSAGE; break; } case STATE_SEND_INFO_MESSAGE: { printf("~im~"); struct infoMessage im; /* See infoMessage.h for description of these info message fields.*/ im.header = hdr; im.deviceType = DEVICETYPE_TESLA_CONTROLS_END_DEVICE_DEMO; im.header.sequence = sequenceNumber++; im.numParameters = getSensorValues(im.kvps); // Does two things: Loads infoMessage with sensor value KVPs and gets the number of them // now, add status message interval im.kvps[im.numParameters].oid = OID_STATUS_MESSAGE_INTERVAL; im.kvps[im.numParameters].value = MESSAGE_PERIOD_SECONDS; im.numParameters++; // add zigbee module information: if (sysVersion() != MODULE_SUCCESS) { printf("ERROR retriving module information\r\n"); } else { displaySysVersion(); // Product ID im.kvps[im.numParameters].oid = OID_MODULE_PRODUCT_ID; im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_PRODUCTID_FIELD]; im.numParameters++; // FW - Major im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_MAJOR; im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_MAJOR_FIELD]; im.numParameters++; // FW - Minor im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_MINOR; im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_MINOR_FIELD]; im.numParameters++; // FW - Build im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_BUILD; im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_BUILD_FIELD]; im.numParameters++; } printInfoMessage(&im); #define RESTART_DELAY_IF_MESSAGE_FAIL_MS 5000 uint8_t messageBuffer[MAX_INFO_MESSAGE_SIZE]; serializeInfoMessage(&im, messageBuffer); setLed(SEND_MESSAGE_LED); //indicate that we are sending a message moduleResult_t result = afSendData(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, 0, INFO_MESSAGE_CLUSTER, messageBuffer, getSizeOfInfoMessage(&im)); // and send it clearLed(SEND_MESSAGE_LED); if (result != MODULE_SUCCESS) { zigbeeNetworkStatus = NWK_OFFLINE; printf("afSendData error %02X; restarting...\r\n", result); delayMs(RESTART_DELAY_IF_MESSAGE_FAIL_MS); //allow enough time for coordinator to fully restart, if that caused our problem state = STATE_MODULE_STARTUP; } else { printf("Success\r\n"); state = STATE_IDLE; } break; } default: //should never happen { printf("UNKNOWN STATE\r\n"); state = STATE_MODULE_STARTUP; } break; } } }
int main(void){ volatile uint32_t delay; int i; PLL_Init(); Output_Init(); // CAN0_Open(); DisableInterrupts(); SYSCTL_RCGCGPIO_R |= 0x20; while((SYSCTL_PRGPIO_R&0x0020) == 0){};// ready? Count = 0; // allow time to finish activating // GPIO_PORTF_DIR_R |= 0x04; // make PF2 out (built-in LED) // GPIO_PORTF_AFSEL_R &= ~0x04; // disable alt funct on PF2 // GPIO_PORTF_DEN_R |= 0x04; // enable digital I/O on PF2 // // configure PF2 as GPIO // GPIO_PORTF_PCTL_R = (GPIO_PORTF_PCTL_R&0xFFFFF0FF)+0x00000000; // GPIO_PORTF_AMSEL_R = 0; // disable analog functionality on PF GPIO_PORTF_LOCK_R = 0x4C4F434B; // 2) unlock GPIO Port F GPIO_PORTF_CR_R = 0x1F; // allow changes to PF4-0 GPIO_PORTF_AMSEL_R = 0x00; // 3) disable analog on GPIO_PORTF_DIR_R &= ~0x03; // make PB6 in GPIO_PORTF_AFSEL_R |= 0x03; // enable alt funct on PB6 GPIO_PORTF_DEN_R |= 0x03; // enable digital I/O on PB6 // configure PB6 as T0CCP0 GPIO_PORTF_PCTL_R = (GPIO_PORTF_PCTL_R&0xFFFFFF00)+0x00000077; GPIO_PORTF_AMSEL_R &= ~0xFF; // disable analog functionality on PB6 SYSCTL_RCGCGPIO_R |= 0x01; delay = SYSCTL_RCGCGPIO_R; // 2) allow time for clock to stabilize delay = SYSCTL_RCGCGPIO_R; GPIO_PORTA_DIR_R |= 0x01; // 3.11) make PA6 output GPIO_PORTA_AFSEL_R &= ~0x01; // 4.11) disable alternate function on PA6 GPIO_PORTA_DEN_R |= 0x01; // 5.11) enable digital I/O on PA6 GPIO_PORTA_AMSEL_R = 0; // 6.11) disable analog functionality on PA6 Switch_Init(); Init_Timer4A(); Timer4A_Wait(80000000); //wait 1 sec Init_Timer5A(80000); ADC0_InitTimer3ATriggerSeq3PD3(6000); ADC0_InitTimer3BTriggerSeq2PD2(5500); TimerCapture_Init(UserTask2); InitMotors(); EnableInterrupts(); while(1) { getSensorValues(); printSensorValues(0); if(buttonL) { printSensorValues(6); ControlMotors(35000,35000); ST7735_SetCursor(0,8);ST7735_OutString("Buttonback"); buttonL = 0; for(i = 0; i < 1440000; i++); } if(buttonR) { printSensorValues(6); ControlMotors(35000,35000); ST7735_SetCursor(0,1);ST7735_OutString("ButtonR"); buttonR = 0; for(i = 0; i < 1440000; i++); } if (Ping1 < 18) { //ControlMotors(40000, 40000); ST7735_SetCursor(0,1); ST7735_OutString("Stopped"); printSensorValues(3); // while(1) { ST7735_SetCursor(0,1);ST7735_OutString("Stopped"); getSensorValues(); printSensorValues(0); // } if(IR_L > IR_R) { ControlMotors(35000,35000); ST7735_SetCursor(0,8);ST7735_OutString("BL"); } else { ControlMotors(35000,35000); ST7735_SetCursor(0,8);ST7735_OutString("BR"); } for(i = 0; i < 1440000; i++); } else if (((Ping2 > 80) != (Ping3 > 80)) && (Ping1 < 40)) { if (Ping2>80) { //spot turn right ControlMotors(20000, 60000); } if (Ping3>80) { //spot turn left ControlMotors(60000, 20000); } // ControlMotors(40000,40000); } else if(IR_L < 1500 && IR_R < 1500) { ControlMotors(70000,65000); ST7735_SetCursor(0,8);ST7735_OutString("FO"); } else if(IR_L < IR_R) { ControlMotors(70000,50000); ST7735_SetCursor(0,8);ST7735_OutString("LE"); } else if(IR_R < IR_L) { ControlMotors(70000,72000); ST7735_SetCursor(0,8);ST7735_OutString("RI"); } } }