/* Function: rtIOStreamClose ================================================
* Abstract: close the connection.
*
*/
int rtIOStreamClose(int streamID)
{
int retVal = RTIOSTREAM_NO_ERROR;
SerialCommsData *sd = getSerialData(streamID);
if (sd == NULL) {
retVal = RTIOSTREAM_ERROR;
return retVal;
}
retVal = serialDataFlush( sd);
#ifdef _WIN32
CloseHandle(sd->serialHandle);
sd->serialHandle = 0;
#else
close(sd->serialHandle);
sd->serialHandle = -1;
/*because unlike Windows which uses a pointer it uses an
int File descriptor which can be 0*/
#endif
/* clear in use flag */
sd->isInUse = 0;
if (sd->verbosity) {
printf("rtIOStreamClose (connection id %d)\n", streamID);
}
return retVal;
}
/* Function: rtIOStreamSend =====================================================
* Abstract:
* Sends the specified number of bytes on the comm line. Returns the number of
* bytes sent (if successful) or a negative value if an error occurred. As long
* as an error does not occur, this function is guaranteed to set the requested
* number of bytes; the function blocks if the TCP/IP send buffer doesn't have
* room for all of the data to be sent
*/
int rtIOStreamSend(
int streamID,
const void * const src,
size_t size,
size_t *sizeSent)
{
int retVal;
SerialCommsData *sd = getSerialData(streamID);
if (sd == NULL) {
retVal = RTIOSTREAM_ERROR;
return retVal;
}
retVal = serialDataSet( sd, src, size, sizeSent);
if (sd->verbosity) {
printf("rtIOStreamSend (connection id %d): size = %lu, sizeSent = %lu",
streamID,
(unsigned long) size,
(unsigned long) *sizeSent);
if (sd->verbosity >= VERBOSITY_LEVEL_2) {
size_t currElement;
printf(": ");
for (currElement = 0; currElement < *sizeSent; currElement++) {
printf("%u ", ((const unsigned char *) src)[currElement]);
}
}
printf("\n");
}
return retVal;
}
/* Function: rtIOStreamRecv ================================================
* Abstract: receive data
*
*/
int rtIOStreamRecv(
int streamID,
void * const dst,
size_t size,
size_t * sizeRecvd)
{
int retVal = RTIOSTREAM_NO_ERROR;
SerialCommsData *sd = getSerialData(streamID);
if (sd == NULL) {
retVal = RTIOSTREAM_ERROR;
return retVal;
}
retVal = serialDataGet( sd, (char *)dst, size, sizeRecvd);
if (sd->verbosity) {
printf("rtIOStreamRecv (connection id %d): size = %lu, sizeRecvd = %lu",
streamID, (unsigned long) size, (unsigned long) *sizeRecvd);
if (sd->verbosity >= VERBOSITY_LEVEL_2) {
size_t currElement;
printf(": ");
for (currElement = 0; currElement < *sizeRecvd; currElement++) {
printf("%u ", ((unsigned char *) dst)[currElement]);
}
}
printf("\n");
}
return retVal;
}
CriusGUI::CriusGUI(QWidget *parent) :
QMainWindow(parent),
ui_(new Ui::CriusGUI),
active_(false),
connected_(false)
{
// Create SerialCommThread
SerialCommThread* serial_thread = new SerialCommThread();
QThread* thread = new QThread;
serial_thread->moveToThread(thread);
connect(thread, SIGNAL(started()), serial_thread, SLOT(init()));
connect(this, SIGNAL(loadFCConfig()), serial_thread, SLOT(requestConfig()));
connect(this, SIGNAL(sendFCConfig(QByteArray)), serial_thread, SLOT(sendConfig(QByteArray)));
connect(this, SIGNAL(sendSerialConfig(QString)),
serial_thread,
SLOT(connectSerial(QString)));
connect(this, SIGNAL(sendSerialDisconnect()), serial_thread, SLOT(disconnectSerial()));
connect(serial_thread, SIGNAL(sendData(QByteArray)), this, SLOT(getSerialData(QByteArray)));
connect(serial_thread, SIGNAL(sendSerialPortInfo(QStringList)), this, SLOT(receiveSerialPortInfo(QStringList)));
connect(this, SIGNAL(sendGotACK()), serial_thread, SLOT(receiveACK()));
thread->start();
// Setup UI
ui_->setupUi(this);
// Initialize plots
imu_plot_= new TimePlot(ui_->imu_plot, ui_->imu_plot->geometry().width()/2, 6);
control_plot_ = new TimePlot(ui_->control_plot, ui_->control_plot->geometry().width()/2, 4);
}
void tempModel(bool updated)
{
// Request Temp information
Serial3.flush();
Serial3.print('T');
delay(200);
if (!updated)
{
data = getSerialData(',');
strcat(data, " F");
tempView(data);
Serial.print("T DATA: ");
Serial.print(data);
Serial.println(" F");
}
else
{
strcpy(screenTitle, "TEMPERATURE");
strcpy(information, "42 C");
strcpy(prevScreen, "main");
strcpy(nextScreen, "altitude");
tempViewUpdate(screenTitle, information, prevScreen, nextScreen);
}
}
void altitudeModel(bool updated)
{
// Request Altitude information
Serial3.flush();
Serial3.print('T');
delay(200);
if (!updated)
{
data = getSerialData('\n');
data = altitudeParse(data);
tempView(data);
Serial.print("Altitude DATA: ");
Serial.print(data);
Serial.println(" cm");
}
else
{
strcpy(screenTitle, "ALTITUDE");
strcpy(information, "42 meters");
strcpy(prevScreen, "temp");
strcpy(nextScreen, "main");
altitudeViewUpdate(screenTitle, information, prevScreen, nextScreen);
}
}
void debugModel(bool updated)
{
// Request Speed information
Serial1.flush();
Serial1.print('S');
if (!updated)
{
data = getSerialData(END_CHAR);
debugView(data);
}
else
{
strcpy(screenTitle, "DEBUG SCREEN");
strcpy(information, "ArduRC Team!");
strcpy(prevScreen, "speed");
strcpy(nextScreen, "main");
debugViewUpdate(screenTitle, information, prevScreen, nextScreen);
}
}
void gpsModel(bool updated)
{
// Request Speed information
Serial1.flush();
Serial1.print('S');
if (!updated)
{
data = getSerialData(END_CHAR);
gpsView(data);
}
else
{
strcpy(screenTitle, "GPS");
strcpy(information, "42\' 35\" 61 N");
strcpy(prevScreen, "altitude");
strcpy(nextScreen, "temp");
gpsViewUpdate(screenTitle, information, prevScreen, nextScreen);
}
}
int main(void)
{
#if 0
int id[NUM_ACTUATOR];
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 512;
//int AmpPos = 2048; // for EX series
int GoalPos;
int i;
int CommStatus;
int isPress = 0;
int isOn = 0;
unsigned char ReceivedData;
int Value;
mServoList[0] = (stServo *)malloc(sizeof(stServo));
memset((void *)mServoList[0], 0x00, sizeof(stServo) );
mServoList[0]->id = 4;
serial_initialize(57600);
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
printf( "\n\nSyncWrite example for CM-700\n\n" );
#ifdef MODE_SYNC
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+2;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
#else
int wPresentPos = 512;
#endif
//Set EEP Lock
dxl_write_word( BROADCAST_ID, P_EEP_LOCK, 1 );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
// Set goal position
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
dxl_write_word( 4, P_TORQUE_LIMIT_L, 0);
_delay_ms(1000);
while(1)
{
if(~PIND & SW_START){
isPress = 1;
}else{
if( isPress == 1 ){
if( isOn == 0 ){
isOn = 1;
}else{
isOn = 0;
}
}
isPress = 0;
}
// while( ReceivedData = getchar() != NULL ){
if(ReceivedData == 'u')
Value++;
else if(ReceivedData == 'd')
Value--;
printf("%d, %d\r\n", Value, ReceivedData);
// }
if( isOn ){
#ifdef MODE_SYNC
// Make syncwrite packet
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
for( i=0; i<NUM_ACTUATOR; i++ )
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);
printf( "%d ", GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
theta += STEP_THETA;
if( theta > 2*PI )
theta -= 2*PI;
#else
wPresentPos = dxl_read_word( 4, P_PRESENT_POSITION_L );
printf( "%d\n", wPresentPos );
dxl_write_word( 2, P_GOAL_POSITION_L, wPresentPos );
dxl_write_word( 3, P_GOAL_POSITION_L, wPresentPos );
PrintErrorCode();
#endif
}
getServoStatus();
_delay_ms(CONTROL_PERIOD);
}
return 0;
#endif
#if 0
DDRC = 0x7F;
PORTC = 0x7E;
DDRD = 0x70;
PORTD = 0x11;
while (1)
{
if(~PIND & SW_START)
PORTC = ~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY);
else PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
}
return 1;
#endif
while(isFinish == 0){
_delay_ms(500);
getSerialData();
// ReceivedData = getchar();
//if(ReceivedData == 'u'){
//printf("%d\r\n", Value);
//Value++;
//}else if(ReceivedData == 'd'){
//printf("%d\r\n", Value);
//Value--;
//}else if(ReceivedData == 10 || ReceivedData == 13 ){
//printf("%s\r\n", "end");
//break;
//}
printf("%s\r\n", "Loop");
}
printf("%s\r\n", "finish");
return 0;
}
/* Returns the next command parsed if any or NULL */
SerialCommand * SerialProtocolHandler::getNextCommand() {
//Get last byte (do it byty by byte)
getSerialData();
return parseLine();
}
