/*
* main.c
* Initializes the robot
* Waits for a new IR packet to be recieved, then handles the request
*/
void main(void) {
initMSP430(); //initialize system
initIR();
P1DIR |= BIT0 | BIT6; // Enable updates to the LED
P1OUT &= ~(BIT0 | BIT6); // An turn the LED off
GO_STOP;
while(1){
if(packetIndex == 34){
handlePress();
_delay_cycles(LONG_T);
initIR();
}
}
}
void KinectInterfacePrimesense::initOpenNI(const char* device_uri) {
initOpenNIStatic();
if (device_uri) {
cout << "Initializing device " << device_uri << "..." << endl;
} else {
cout << "Initializing first avaliable openNI device..." << endl;
}
// Open a connection to the OpenNI device
const char* deviceURI = openni::ANY_DEVICE;
if (device_uri != NULL) {
deviceURI = device_uri;
}
device_ = new openni::Device();
checkOpenNIRC(device_->open(deviceURI), "Failed to connect to device");
const openni::DeviceInfo& info = device_->getDeviceInfo();
device_uri_ = info.getUri();
// Note: XYZ (real world values are ALL wrong when image registration is
// turned on). It looks OK for a single Kinect, but the space is warped.
// This is an OpenNI bug:
setCropDepthToRGB(false);
setDepthColorSync(false);
// Open a connection to the device's depth channel
initDepth();
const int depth_size = depth_dim_[0] * depth_dim_[1];
labels_ = new uint8_t[depth_size];
pts_world_ = new float[3 * depth_size];
pts_uvd_ = new float[3 * depth_size];
registered_rgb_ = new uint8_t[3 * depth_size];
// Open a connection to the device's rgb channel
bool sync_ir_stream_ = false;
initRGB(!sync_ir_stream_);
initIR(sync_ir_stream_);
bool flip_image_ = true;
setFlipImage(flip_image_);
std::this_thread::sleep_for(std::chrono::milliseconds(1));
// Update the OpenNIFuncs constants
float depth_hfov = streams_[DEPTH_STREAM]->getHorizontalFieldOfView();
float depth_vfov = streams_[DEPTH_STREAM]->getVerticalFieldOfView();
openni_funcs_ = new OpenNIFuncs(depth_dim_[0], depth_dim_[1],
depth_hfov, depth_vfov, openni_devices_open_);
// Check OpenNI's math:
//uint64_t zpd;
//streams_[DEPTH_STREAM]->getProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, &zpd);
//double zpps;
//streams_[DEPTH_STREAM]->getProperty(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, &zpps);
//double focal_length_cmos = (double)zpd / zpps;
//// http://en.wikipedia.org/wiki/Angle_of_view FOV calculation
//double FOVh = 2.0 * atan( ( 1280.0 * 0.5 ) / focal_length_cmos );
//double FOVv = 2.0 * atan( ( 960.0 * 0.5 ) / focal_length_cmos );
//std::streamsize old_precision = std::cout.precision();
//std::cout.precision(15);
//std::cout << "FOVh = " << FOVh << std::endl;
//std::cout << "FOVv = " << FOVv << std::endl;
//std::cout << "depth_hfov = " << depth_hfov << std::endl;
//std::cout << "depth_vfov = " << depth_vfov << std::endl;
//std::cout.precision(old_precision);
std::cout << "Finished initializaing OpenNI device" << std::endl;
}
/*
* handlePress()
* copied from Lab05
* decodes which button was pressed on remote
* modified to call appropriate drive function
*/
void handlePress(){
_disable_interrupt();
int32 result = 0;
int32 setter = 0x80000000; //1 in the MSB
char i;
for(i = 2; i<34; i++){ //traverse array
int16 current = packetData[i]; //current element
packetData[0] = 0; //clear element
if(current/10 < 100){ //is a zero
result &= ~setter; //clear bit
}
else {
result |= setter; //set bit
}
setter >>= 1; //rotate setter
}
packetIndex++; //ensure the loop is not reentered
switch(result){ //take appropriate action
case UP:
P1OUT ^= BIT0;
drive(FORWARD);
break;
case DOWN:
P1OUT ^= BIT6;
drive(BACKWARD);
break;
case LEFT:
P1OUT |= (BIT0 | BIT6);
drive(LEFT_T);
break;
case RIGHT:
P1OUT ^= (BIT0 | BIT6);
drive(RIGHT_T);
break;
case EXIT:
drive(STOP);
break;
case CH_UP:
break;
case CH_DW:
break;
case MUTE:
break;
}
initIR();
}
int main(void)
{
sysInfo = 0;
SCS |= 0x01;
FIODIR0 |= ((1<<21) | (1<<4) | (1<<11) | (1<<6) | (1<<23) | (1<<19) | (1<<17));
FIODIR0 |= (1<<12);
FIOSET0 |= (1<<12);
xx = 0x00;
setSpeed(SPEED_30);
lcd_init(0);
serial_init();
startTimerIRQ();
startADC();
initKeys();
initSound();
startSoundIRQ();
initIR();
startIrIRQ();
RF_init();
startcc1100IRQ();
enableWOR();
initRTC();
startRtcIRQ();
enableIRQ();
testmenu_init();
init_menu();
initBacklight();
oldkeys[0] = keys[0];
oldkeys[1] = keys[0];
key_state = KEY_IDLE;
set_font(BOLDFONT);
BFS_Mount();
load_RC_setting();
load_RF_setting();
load_setting();
{
struct RAWset_ RAWset;
unsigned char x;
unsigned long RAWcmdbase;
RAWcmdbase = FLASH1_BASE +(secaddr[0]<<1);
x=memcmp((void*)RAWcmdbase,"RC01",4);
if(!x) {
memcpy(&RAWset,(void *)RAWcmdbase,sizeof(struct RAWset_));
RAWset.name[7] = 0;
BFS_SaveFile(BFS_ID_RAWslot0, sizeof(struct RAWset_), (unsigned char*) &RAWset);
eraseSector(1,0);
}
}
if (EncIsValid(irDevTab.device[irDevTab.active].encoder, irDevTab.device[irDevTab.active].set)) {
setEncoder(irDevTab.device[irDevTab.active].encoder, irDevTab.device[irDevTab.active].set);
}
drawMainscreen();
ask_for_time(0);
/*
playSound((unsigned char*)sound1_data, sound1_len);
waitSound();
playSound((unsigned char*)sound2_data, sound2_len);
*/
while (1)
{
if(keys[0] != oldkeys[0] || keys[1] != oldkeys[1])
{
oldkeys[0] = keys[0];
oldkeys[1] = keys[1];
sysInfo |= 0x40;
}
switch(key_state)
{
case KEY_IDLE:
if(sysInfo & 0x40)
{
sysInfo &= 0xBF;
if(KEY_Betty)
{
setBacklight(BL_AUTO);
menu_exec(&mainMenu);
if (EncIsValid(irDevTab.device[irDevTab.active].encoder, irDevTab.device[irDevTab.active].set)) {
setEncoder(irDevTab.device[irDevTab.active].encoder, irDevTab.device[irDevTab.active].set);
}
drawMainscreen();
}
else if(KEY_2)
{
//setSpeed(SPEED_30);
}
else if(KEY_3)
{
//setSpeed(SPEED_60);
}
else if(KEY_A || KEY_B || KEY_C || KEY_D)
{
unsigned char x;
//playSound((unsigned char*)sound3_data, sound3_len);
x=0;
if (KEY_B) x=1;
if (KEY_C) x=2;
if (KEY_D) x=3;
if (EncIsValid(irDevTab.device[x].encoder, irDevTab.device[x].set)) {
setBacklight(BL_AUTO);
irDevTab.active = x;
setEncoder(irDevTab.device[x].encoder, irDevTab.device[x].set);
drawMainscreen();
}
}
/* else if(KEY_B)
{
}
else if(KEY_C)
{
// playSound((unsigned char*)sound1_data, sound1_len);
}
else if(KEY_D)
{
// playSound((unsigned char*)sound2_data, sound2_len);
}*/
if((keys[0] != 0) || (keys[1] != 0))
key_state = KEY_PRESS;
}
break;
case KEY_PRESS:
irSend(getCode());
key_state = KEY_HOLD;
// autorepeat = 0;
break;
case KEY_HOLD:
// if(autorepeat >= AUTO_TIMEOUT)
irRepeat();
if(keys[0] == 0 && keys[1] == 0)
key_state = KEY_RELEASE;
break;
case KEY_RELEASE:
irStop();
key_state = KEY_IDLE;
break;
}
if(serial_tstc() > 0)
{
i = serial_getc();
if(i=='.')
{
serial_puts("HELO");
}
else if(i=='0')
{
setBacklight(0x00); // pwm value
}
else if(i=='1')
{
setBacklight(0x1F); // pwm value
}
else if(i=='2')
{
setBacklight(0x3F); // pwm value
}
else
serial_putc(i);
}
if ((bl_val == 0) && (key_state == KEY_IDLE) && !((FIOPIN0 & (1<<30)) == 0) && (((RFstatus & (WORrxon | RXenabled)) == 0))) {
EXTINT = 0x08;
PCON = 0x02;
PLLFEED = 0xAA;
PLLFEED = 0x55;
}
if (timeInfo & timechanged) {
printTime(86,152,(struct time_ *)&time);
printDate(0,152,(struct date_ *)&date,0);
timeInfo &= ~timechanged;
}
}
return 0;
}
