RawAngle HardwareSensor::getRawAngle()
{
// Send a SPI request for angle data, ignoring the result as it belongs
// to the previously issued command.
sendReceive(CMD_ANGLEDATA | FLAG_READ);
// Flush the command with a NOOP and record the reply.
uint16_t angledata = sendReceive(CMD_NOOP);
return RawAngle((degrees)(angledata & 0x3fff) * 360.0f / 0x3fff);
}
double Bisight::getJointPosition(int jnt) {
char cmd[4] = " ";
//ROS_INFO("Requesting pos for joint: %d", jnt);
switch(jnt) {
case PAN:
cmd[0] = 'p';
break;
case TILT:
cmd[0] = 't';
break;
case VERGE_LEFT:
cmd[0] = 'l';
break;
case VERGE_RIGHT:
cmd[0] = 'r';
break;
default:
return 0;
break;
}
//ROS_INFO("Command: %s", cmd);
sendReceive(cmd, buffer);
int data[4];
parseReply(buffer, data);
return ((double)data[0] / (double)RAD_TO_ENC);
}
IFvDataSource * createRemoteFileDataSource(const SocketEndpoint & server, const char * username, const char * password, const char * logicalName)
{
Owned<INode> serverNode = createINode(server);
CMessageBuffer msg;
msg.setEndian(__BIG_ENDIAN);
msg.append((byte)FVCMDcreatefile);
msg.append(myProcessSession());
msg.append(username);
msg.append(password);
msg.append(logicalName);
sendReceive(serverNode, msg);
unsigned short version;
unique_id_t id;
__int64 numRows;
bool isIndex;
msg.read(version);
msg.read(id);
msg.read(numRows);
Owned<IFvDataSourceMetaData> meta = deserializeDataSourceMeta(msg);
msg.read(isIndex);
if (id)
return new RemoteDataSource(server, id, meta, numRows, isIndex);
return 0;
}
bool RemoteDataSource::getARow(MemoryBuffer & out, RowCache & cache, byte cmd, __int64 row)
{
RowLocation location;
if (cache.getCacheRow(row, location))
{
out.append(location.matchLength, location.matchRow);
return true;
}
CMessageBuffer msg;
msg.setEndian(__BIG_ENDIAN);
msg.append(cmd);
msg.append(id);
msg.append(row);
sendReceive(msg);
bool ok;
msg.read(ok);
if (!ok) return false;
__int64 start;
msg.read(start);
VariableRowBlock * next = new VariableRowBlock(msg, start);
cache.addRowsOwn(next);
if (!cache.getCacheRow(row, location))
assertex(!"Internal Error!");
out.append(location.matchLength, location.matchRow);
return true;
}
ae_error_t CertificateProvisioningProtocol::SendM3_ReceiveM4
( /*in */ const upse::Buffer& csrBuffer,
/*in */ const upse::Buffer& quoteBuffer,
/*out*/ std::list< upse::Buffer >& certificateChainList,
/*out*/ platform_info_blob_wrapper_t& piBlobWrapper
)
{
ae_error_t status = AE_FAILURE;
upse::Buffer serializedMsg3;
upse::Buffer serializedMsg4;
AESM_DBG_TRACE("start to send M3");
do
{
BREAK_IF_FALSE((m_is_initialized), status, AESM_PSE_PR_BACKEND_NOT_INITIALIZED);
BREAK_IF_FALSE( (msg_next_state_M3 == m_nextState), status, AESM_PSE_PR_CALL_ORDER_ERROR);
status = msg3_generate(csrBuffer, quoteBuffer, serializedMsg3);
BREAK_IF_FAILED_ERR(status, AESM_PSE_PR_BACKEND_MSG3_GENERATE);
// BREAK_IF_FAILED(status);
AESM_DBG_TRACE("M3 generated");
status = sendReceive(serializedMsg3, serializedMsg4);
BREAK_IF_FAILED(status);
AESM_DBG_TRACE("start to process M4");
status = msg4_process(serializedMsg4, certificateChainList, piBlobWrapper);
BREAK_IF_FAILED(status);
AESM_DBG_TRACE("finished M4");
} while (0);
m_nextState = msg_next_state_init;
return status;
}
void Client::getMail(const unsigned int i) {
// convert integer value to string
std::stringstream ss;
ss << i;
std::string message = "RETR " + ss.str() + "\n";
try {
sendReceive(message); // status message
receiveMessage(message); // data
}
catch (const char * e) {
std::string tmp(e);
if (tmp == "Error response") {
std::cerr << "Can't get message " << i << std::endl;
throw e;
}
}
catch (...) {
std::cerr << "An error occured during receiving message " << i << std::endl;
}
if (shortMessage) {
// print message without header and last termination octet
const size_t pos = message.find("\r\n\r\n");
std::cout << message.substr(pos+4, message.length() - pos-4 -3 ); // last "-3" is to remove last ".\r\n"
}
else
std::cout << message;
}
ae_error_t CertificateProvisioningProtocol::SendM1_ReceiveM2
( /*in */ const uint32_t gid,
/*out*/ upse::Buffer& nonce,
/*out*/ upse::Buffer& sigRLBuffer
)
{
ae_error_t status = AE_FAILURE;
upse::Buffer serializedMsg1;
upse::Buffer serializedMsg2;
do
{
BREAK_IF_FALSE((m_is_initialized), status, AESM_PSE_PR_BACKEND_NOT_INITIALIZED);
BREAK_IF_FALSE( (msg_next_state_M1 == m_nextState), status, AESM_PSE_PR_CALL_ORDER_ERROR);
status = msg1_generate(*(const GroupId*)&gid, serializedMsg1);
BREAK_IF_FAILED_ERR(status, AESM_PSE_PR_BACKEND_MSG1_GENERATE);
// BREAK_IF_FAILED(status);
status = sendReceive(serializedMsg1, serializedMsg2);
if (AE_FAILED(status))
break;
status = msg2_process(serializedMsg2, nonce, sigRLBuffer);
if (AE_FAILED(status))
break;
m_nextState = msg_next_state_M3;
} while (0);
return status;
}
void Client::quit() {
try {
sendReceive("QUIT\n");
close(sockfd);
}
catch (...) {
// an error occured but we don't care, we are quitting anyway
}
}
void RemoteDataSource::beforeDispose()
{
CMessageBuffer msg;
msg.setEndian(__BIG_ENDIAN);
msg.append((byte)FVCMDdestroy);
msg.append(id);
sendReceive(msg);
}
int Bisight::getHomeValue() {
char cmd[4] = "h";
if (sendReceive(cmd, buffer)) {
int data[4];
parseReply(buffer, data);
return data[0];
}
return -1;
}
void Bisight::home() {
printf("Homing Bisight");
char cmd[4] = "H";
sendReceive(cmd, buffer);
printf("Reply from Home = %s\n", buffer);
//int data[4];
//parseReply(Reply, data);
}
void Client::login(const std::string& user) {
// печатаем пароль
char *pass = getpass("Enter password (won't be printed):");
std::string response;
// USER
try {
response = sendReceive("USER " + user + "\n");
}
catch (const char * e) {
std::string tmp(e);
if (tmp == "Error response") {
// USER command can receive an -ERR message even if the user exists! (security reasons).
// If we receive -ERR for USER command, we ignore it
}
else throw e;
}
catch (...) {
std::cerr << "An error occured during login" << std::endl;
throw "Login error";
}
// PASS
try {
response = sendReceive("PASS " + (std::string)pass + "\n");
}
catch (const char * e) {
std::string tmp(e);
if (tmp == "Error response") {
std::cerr << "Login is unsuccessful" << std::endl;
throw e;
}
}
catch (...) {
std::cerr << "An error occured during login" << std::endl;
throw "Login error";
}
}
__int64 RemoteDataSource::numRows(bool force)
{
if (!force)
return cachedNumRows;
CMessageBuffer msg;
msg.setEndian(__BIG_ENDIAN);
msg.append((byte)FVCMDnumrows);
msg.append(id);
sendReceive(msg);
__int64 result;
msg.read(result);
return result;
}
void Bisight::getJointPositions(double *value) {
char cmd[32];
sprintf(cmd, "a");
sendReceive(cmd, buffer);
int data[4];
parseReply(buffer, data);
int i;
for (i=0; i<4; i++) {
value[i] = (double)data[i] / (double)RAD_TO_ENC;
}
}
bool RemoteDataSource::fetchRawRow(MemoryBuffer & out, __int64 offset)
{
CMessageBuffer msg;
msg.setEndian(__BIG_ENDIAN);
msg.append(FVCMDfetchraw);
msg.append(id);
msg.append(offset);
sendReceive(msg);
bool ok;
msg.read(ok);
if (!ok) return false;
size32_t len;
msg.read(len);
out.append(len, msg.readDirect(len));
return true;
}
void Bisight::park() {
char cmd[4] = "Q";
if(getHomeValue() == 1) {
mode = PARKED;
printf("Parking Bisight...\n");
sendReceive(cmd, buffer);
printf("Reply from Park = %s\n", buffer);
//int data[4];
//parseReply(buffer, data);
}
else{
printf("Bisight needs to be homed before it can be parked!");
}
}
void Client::listMails() {
std::string message = "LIST\n";
sendReceive(message); // status message
receiveMessage(message); // data
if (message.length() == 3) { // = ".CRLF"
std::cout << "No new messages" << std::endl;
return;
}
if (shortMessage) {
// remove last dot + CRLF
message.erase( message.length() - 3, 3);
}
std::cout << message;
}
/**
* Sets the desired joint position
* @param jnt the joint number
* @param value the position in radians
*/
void Bisight::setJointPosition(int jnt, double val) {
char cmd[32];
int value[1];
//clamping of position commands to joint limits
if (val > limit_max[jnt])
val = limit_max[jnt];
if (val < limit_min[jnt])
val = limit_min[jnt];
value[0] = (int)(val * (double)RAD_TO_ENC);
switch(jnt) {
case PAN:
sprintf(cmd, "P");
createCommand(cmd, value, 1);
break;
case TILT:
sprintf(cmd, "T");
createCommand(cmd, value, 1);
break;
case VERGE_LEFT:
sprintf(cmd, "L");
createCommand(cmd, value, 1);
break;
case VERGE_RIGHT:
sprintf(cmd, "R");
createCommand(cmd, value, 1);
break;
default:
break;
}
sendReceive(cmd, buffer);
//int data[4];
//parseReply(buffer, data);
}
void PetscParallelManagerTurbulent::communicateViscosity() {
_viscosityBufferFillIterator.iterate();
// Left to right & Right to left
sendReceive(_viscositySendBufferRightWall, _parameters.parallel.rightNb, _viscosityRecvBufferLeftWall, _parameters.parallel.leftNb, _cellsLeftRight);
sendReceive(_viscositySendBufferLeftWall, _parameters.parallel.leftNb, _viscosityRecvBufferRightWall, _parameters.parallel.rightNb, _cellsLeftRight);
// Top to bottom & Bottom to top
sendReceive(_viscositySendBufferTopWall, _parameters.parallel.topNb, _viscosityRecvBufferBottomWall, _parameters.parallel.bottomNb, _cellsTopBottom);
sendReceive(_viscositySendBufferBottomWall, _parameters.parallel.bottomNb, _viscosityRecvBufferTopWall, _parameters.parallel.topNb, _cellsTopBottom);
// Front to back & Back to front
sendReceive(_viscositySendBufferFrontWall, _parameters.parallel.frontNb, _viscosityRecvBufferBackWall, _parameters.parallel.backNb, _cellsFrontBack);
sendReceive(_viscositySendBufferBackWall, _parameters.parallel.backNb, _viscosityRecvBufferFrontWall, _parameters.parallel.frontNb, _cellsFrontBack);
_viscosityBufferReadIterator.iterate();
}
/**
* Sets the desired joint position
* @param jnt the joint number
* @param value the position in radians
*/
void Bisight::setJointPositions(double *val) {
int i;
for (i=0; i<4; i++) {
if (val[i] > limit_max[i])
val[i] = limit_max[i];
if (val[i] < limit_min[i])
val[i] = limit_min[i];
}
char cmd[32];
int value[4];
value[0] = (int)(val[0] * (double)RAD_TO_ENC);
value[1] = (int)(val[1] * (double)RAD_TO_ENC);
value[2] = 0;
value[3] = 0;
sprintf(cmd, "A");
createCommand(cmd, value, 4);
sendReceive(cmd, buffer);
//int data[4];
//parseReply(buffer, data);
}
void myfunc() {
int num_rows,k, l;
double orient,delta_orient,rot_speed,t_rot,t_trans;
double x0,y0,x,y,dx,dy;
float distance,distance1,lala;
double PI=3.14159265;
x0=0;
y0=0;
double orient0;
orient0=0; // starting conditions still have to be properly intialized
double D = 0.094; /*distance between wheels, has to be measured*/
char ch;
const float tol = 1.0e-7; // 7 significant figures
float xold, xnew; // local variables
int test;
/*Read in coords, mat[d][0] contains x-coords, mat[d][1] contains y-coords*/
int i=0,j=0;
double mat[100][2];
double z;
// UInt32* pathx;
// UInt32* pathy;
// Here we should store the path-coordinates
//
// while((line=fgets(buffer,sizeof(buffer),fp))!=NULL)
// {
//
// record = strtok(line,","); /*break up string, return first token */
// while(record != NULL)
// {
// printf("record : %s \n",record); //here you can put the record into the array as per your requirement.
// mat[i][j++] = atof(record) ; /*Convert string to double, put in [i,j] then j+1*/
// record = strtok(NULL,",");
// }
// ++i ;
// j=0;
// }
initRTC();
camera_reset(160);
init_uart1(57600);
while(1){
if(getsignal()){
ch = getch();
printf("Character on serial %c 1\r\n", ch );
if(ch == 75)
{return;}
else{
switch (ch) {
case '1':
/*Read in csv-file, mat[d][0] contains distance in m, mat[d][1] contains delta_orientation in degrees */
mat[0][0]=0.61;
mat[0][1]=0;
mat[1][0]=0.58;
mat[1][1]=90;
mat[2][0]=1.00;
mat[2][1]=-45;
mat[3][0]=0.33;
mat[3][1]=-60;
num_rows = 4; // to be determined depends on how weread in givens
k=0;
while(k<=num_rows-1) {
distance=mat[k][0];
delta_orient=mat[k][1];
if(delta_orient>0){
t_rot=1000*(0.010*delta_orient+0.003);
}
else{
if(delta_orient<0){
t_rot=1000*(-0.010*delta_orient+0.003);
}
else t_rot=0;
}
t_trans=1000*(4.8*distance+0.05);
printf("tekst 2 distance: %lf,delta_orient:%lf \n t_rot: %lf t_trans: %lf \n\n",distance*1000,delta_orient,t_rot,t_trans);
if (delta_orient>0){
sendReceive(203,22,202,20);//turn counterclockwise
delay(t_rot);
sendReceive(201,0,202,0);//stop
}
if(delta_orient<0){
sendReceive(201, 22, 204, 20);//turn clockwise
delay(t_rot);
sendReceive(201,0,202,0);//stop
}
sendReceive(201, 23, 202, 20);//drive forward
delay(t_trans);
sendReceive(201, 0, 202, 0);//stop
printf("k: %d",k);
k++;
}
break;
case '5': /* Drive forward */
sendReceive(201, 23, 202, 20);
/* 201: Right motor moves forward
202: Left motor moves forward
Forward= opposite the antenna
*/
break;
case '6': /* Stop */
sendReceive(201, 0, 202, 0);
break;
case '7':
sendReceive(203, 22, 204, 20);
/* 203: Right motor moves backward
204: Left motor moves backward
Backward= antenna - side
*/
break;
case '8': /* Rotate left (counter clockwise) */
sendReceive(203, 22, 202, 20);
/* Rotate: left motor rotates backward; right one forward */
break;
case '9': /* Rotate right (clockwise) */
sendReceive(201,22,204,20);
/* Rotate: left motor rotates forward; right one backward */
break;
case 'a':
printf("Reading path");
l = readNumber();
printf("l = ",l);
followPath(l);
break;
}
}
}
} //end while
} //end myfunc
