//======================================================================
// Get a whole block. Handles checksums, ...
//======================================================================
static int
getBlock(SerialPort& port, char* block)
{
int checksum;
int ch;
int blockNum;
for (int errorCnt = 0; errorCnt < RETRY_MAX; errorCnt++) {
ch = recvChar(port, 2);
if (ch == EOT) {
sendChar(port, ACK);
return GOT_EOT;
}
if (ch == CAN) {
exitCode = 7;
return ERROR;
}
if (ch == SOH || ch == STX) {
if (ch == SOH)
blockLen = 128;
else
blockLen = 1024;
blockNum = recvChar(port, 2);
ch = recvChar(port, 2);
if (blockNum + ch == 0377) {
checksum = 0;
char* p = block;
for (int cnt = 0; cnt < blockLen; cnt++) {
ch = recvChar(port, 2);
checksum += (*p++ = ch);
}
ch = recvChar(port, 2);
if (((checksum - ch) & 0377) == 0)
return blockNum;
exitCode = 8;
}
}
if (ch == TIMEOUT)
exitCode = 9;
flushInput(port);
sendChar(port, NAK);
}
return ERROR;
}
static void
flushInput(SerialPort& port)
{
int ch;
do {
ch = recvChar(port, 2);
} while (ch != TIMEOUT);
}
void mod_comms_service()
{
char data;
switch(mc_comm_mode)
{
case MC_IDLE:
if(isChar())
{
data = recvChar();
// parse command
switch(data)
{
case 'B':
triggerBeepTone(2);
break;
case 'C':
if(mc_count > 9) {
mc_count = 0;
}
sendChar('@');
sendChar(mc_count + '0');
sendChar(13); // CR
sendChar(10); // LF
++mc_count;
break;
case 'G':
mod_io_setBlink(100, IO_GREEN_LED);
break;
case 'R':
mod_io_setBlink(100, IO_RED_LED);
break;
default:
break;
}
}
break;
default:
mc_comm_mode = MC_IDLE;
break;
}
}
int main(int argc, char* argv[]) {
unsigned char ret;
char c;
char len;
char i;
char* annoyname = "Annoy_Light";
char* stairname = "Stair_Light";
DDRC = 0;
PORTC = 0x7;
unsigned char did = 1;
// define direction of inputs and outputs
// 1 for output, 0 for input
// | for output, & for input
DDRD = (1<<RELAY_STAIRS);
DDRD |= (1<<RELAY_ANNOY);
did = PINC & 0x7;
serial_init();
buf_index = 0;
transmit_flag = 0;
// FILE s=FDEV_SETUP_STREAM(send_char_serial,NULL,_FDEV_SETUP_WRITE);
// stdout=&s;
// sei();
Device* annoydevice = createDevice(annoyname, 1, buffered_send_char);
Device* stairdevice = createDevice(stairname, 1, buffered_send_char);
setDeviceCName("Annoying Light", annoydevice);
setDeviceCName("Stairs Light", stairdevice);
setDeviceLocation(0, 0, 0, 0, annoydevice);
setDeviceLocation(0, 0, 0, 0, stairdevice);
addField(BOOL, "Light Status", 0, 1, 0, annoydevice);
addField(BOOL, "Light Status", 0, 1, 0, stairdevice);
while (1) {
c = receive_char_serial();
if (c == '\0') {
if (receive_char_serial() == did) {
serial_tx_enable();
if (transmit_flag) {
len = buf[0] + 1;
// printf("sending packet %d\n", buf[3]);
for (i = 0; i < len; i++) {
send_char_serial(buf[i]);
}
// printf("bi:%d %d %d\n", buf_index, len, buf[3]);
for (i = len; i < buf_index; i++) {
buf[i-len] = buf[i];
}
buf_index -= len;
if (buf_index == 0) {
transmit_flag = 0;
}
} else {
send_char_serial('\0');
}
serial_tx_disable();
}
} else {
len = c;
recvChar(len, annoydevice);
recvChar(len, stairdevice);
for (i = 0; i < len; i++) {
c = receive_char_serial();
recvChar(c, annoydevice);
recvChar(c, stairdevice);
if (shouldBreak(annoydevice)) break;
if (shouldBreak(stairdevice)) break;
}
}
/*
if (((r_front + 1) % MAX_LEN) != r_back) {
r_front++;
if (r_front == MAX_LEN) {
r_front = 0;
}
recvChar(r_buf[r_front], device);
}
*/
if (hasChanged(0, annoydevice)) {
ret = getBoolVal(0, annoydevice);
if (ret) {
RELAY_ANNOY_ON();
} else {
RELAY_ANNOY_OFF();
}
}
if (hasChanged(0, stairdevice)) {
ret = getBoolVal(0, stairdevice);
if (ret) {
RELAY_STAIRS_ON();
} else {
RELAY_STAIRS_OFF();
}
}
}
return 0;
}
void Joystick::update()
{
if(connected){
serial.writeByte('0');
char recv;
bool somethingIn = false;
while(serial.available())
{
recv = serial.readByte();
recvChar(recv);
somethingIn = true;
}
if(somethingIn && ofGetElapsedTimeMillis() > 4000){
for(int i=0;i<16;i++){
// cout<<i<<" "<<returnedFlags[i]<<endl;
}
//cout<<returnedFlags[8]<<endl;
if(!returnedFlags[8]){
if(!button8down){
button8down ++;
//if(button8down == 1){
robot->unlockMotors->toggle();
}
// }
} else {
button8down = 0;
}
/* if(!returnedFlags[7]){
cout<<"RIIING "<<ofGetElapsedTimeMillis()<<endl;
}*/
if(returnedFlags[5]){
emergencyDown ++;
if(emergencyDown == 10){
robot->unlockMotors->setChecked(false);
robot->industrialRobot->panic("Emergency stop on joystick");
}
} else
emergencyDown = 0;
if(returnedFlags[4] && !returnedFlags[2]){
// if(robot->industrialRobot->thread.lock()){
if(robot->industrialRobot->thread.controller->input == ofxIndustrialRobotController::ManualPosition){
if(returnedFlags[3]){
robot->manualPosition[0]->setValue(robot->manualPosition[0]->value() - joystick[0]*0.005) ;
robot->manualPosition[1]->setValue(robot->manualPosition[1]->value() + joystick[1]*0.005) ;
robot->manualPosition[2]->setValue(robot->manualPosition[2]->value() + joystick[2]*0.005) ;
} else {
robot->manualPosition[3]->setValue(robot->manualPosition[3]->value() - joystick[0]*0.0001) ;
robot->manualPosition[4]->setValue(robot->manualPosition[4]->value() + joystick[1]*0.0001) ;
robot->manualPosition[5]->setValue(robot->manualPosition[5]->value() + joystick[2]*0.0001) ;
}
}
/* if(robot->industrialRobot->thread.controller->input == ofxIndustrialRobotController::Slider){
if(returnedFlags[3]){
robot->motorSlider[1]->setValue(robot->motorSlider[1]->value() - joystick[0]*0.005);
robot->motorSlider[2]->setValue(robot->motorSlider[2]->value() + joystick[1]*0.005);
// robot->manualPosition[2]->setValue(robot->manualPosition[2]->value() + joystick[2]*0.005) ;
} else {
robot->motorSlider[1]->setValue(robot->motorSlider[1]->value() - joystick[0]*0.05);
robot->motorSlider[2]->setValue(robot->motorSlider[2]->value() + joystick[1]*0.05);
}
cout<<robot->motorSlider[1]->value()<<" "<<robot->motorSlider[2]->value()<<endl;
cout<<joystick[0]<<endl;
}*/
// robot->industrialRobot->thread.unlock();
// }
}
}
}
}
