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");
}
}
}
