/**
* A high level function which rotates Create for
* a given angle and with given speed. The signs of
* parameters should be same
* @param angle an integer value in degrees.
* Range: -32768 – 32767
* @param speed an integer value. Range -500-500
*/
int Create::rotate(int angle, int speed) {
if (angle*speed < 0) return EXIT_FAILURE; //sign test
std::vector<char> input;
if (speed < VELOCITY_MIN)
speed = VELOCITY_MIN;
else if (speed > VELOCITY_MAX)
speed = VELOCITY_MAX;
input.push_back(DRIVE_DIRECT);
input.push_back(intToBytes((short int) speed, 1));
input.push_back(intToBytes((short int) speed, 0));
input.push_back(intToBytes((short int) -speed, 1));
input.push_back(intToBytes((short int) -speed, 0));
if (angle < ANGLE_MIN)
angle = ANGLE_MIN;
else if (angle > ANGLE_MAX)
angle = ANGLE_MAX;
input.push_back(WAIT_ANGLE);
input.push_back(intToBytes((short int) angle, 1));
input.push_back(intToBytes((short int) angle, 0));
input.push_back(DRIVE_DIRECT);
input.push_back(0);
input.push_back(0);
input.push_back(0);
input.push_back(0);
return writePort(input);
}
/************************************************************
* startRobot - send a command to the sample changer
* returns immediately without waiting for the sample changer to complete
*/
int startRobot( char cmd, int smpnum )
{
char *ptr;
char chrbuf[20];
char chrbuf1[128];
int i, wbyte, rbyte, stat;
tcflush(serialPort,TCIFLUSH); /* clear serial port */
sprintf(chrbuf,"%c", cmd);
ptr = chrbuf;
/* Sending alphabetical part of command to robot */
wbyte= write(serialPort, ptr, 1);
stat = readPort(serialPort, 0, 6);
if (stat == SMPTIMEOUT) /* and read the echo back till end of text "0x03" */
return(stat);
/*-- these commands require a parameter --*/
if (cmd == 'F' || cmd == 'M' || cmd == 'A' || cmd == 'V')
{
writePort(serialPort,smpnum);
}
write(serialPort, "\r",1); /* Sending "cr" , The end of command */
return (0);
}
void writeNetlistInterface(QXmlStreamWriter &stream, const QVariantList &v)
{
stream.writeStartElement("interface");
for (int i = 1; i < v.size(); i++)
{
QVariant v2 = v[i];
if (v2.type() == QVariant::List)
{
QVariantList l2 = v2.toList();
if (l2.size() > 0)
{
QString tag = l2.first().toString();
if (tag == "port")
{
writePort(stream, l2);
}
else
{
qWarning("Unexpected tag '%s' in 'interface' tag", qPrintable(tag));
}
}
}
}
stream.writeEndElement();
}
static int initialize(cchar *ip, int port)
{
if ((app->appweb = maCreateAppweb()) == 0) {
mprUserError("Can't create HTTP service for %s", mprGetAppName());
return MPR_ERR_CANT_CREATE;
}
MPR->appwebService = app->appweb;
if ((app->server = maCreateServer(app->appweb, "default")) == 0) {
mprUserError("Can't create HTTP server for %s", mprGetAppName());
return MPR_ERR_CANT_CREATE;
}
if (maConfigureServer(app->server, app->configFile, app->home, app->documents, ip, port) < 0) {
/* mprUserError("Can't configure the server, exiting."); */
return MPR_ERR_CANT_CREATE;
}
if (app->workers >= 0) {
mprSetMaxWorkers(app->workers);
}
#if BLD_WIN_LIKE
writePort(app->server);
#elif BLD_UNIX_LIKE
app->traceToggle = mprAddSignalHandler(SIGUSR2, traceHandler, 0, 0, MPR_SIGNAL_AFTER);
#endif
return 0;
}
/**
* A high level function which rotates Create for
* a given distance with given speed. The signs of
* parameters should be same
* @param distance an integer value in mm.
* Range: -32768 – 32767
* @param speed an integer value. Range -500-500
*/
int Create::translate(int distance, int speed) {
if (distance*speed < 0) return EXIT_FAILURE; //sign test
std::vector<char> input;
if (speed < VELOCITY_MIN)
speed = VELOCITY_MIN;
else if (speed > VELOCITY_MAX)
speed = VELOCITY_MAX;
input.push_back(DRIVE_DIRECT);
input.push_back(intToBytes((short int) speed, 1));
input.push_back(intToBytes((short int) speed, 0));
input.push_back(intToBytes((short int) speed, 1));
input.push_back(intToBytes((short int) speed, 0));
if (distance < DISTANCE_MIN)
distance = DISTANCE_MIN;
else if (distance > DISTANCE_MAX)
distance = DISTANCE_MAX;
input.push_back(WAIT_DISTANCE);
input.push_back(intToBytes((short int) distance, 1));
input.push_back(intToBytes((short int) distance, 0));
input.push_back(DRIVE_DIRECT);
input.push_back(0);
input.push_back(0);
input.push_back(0);
input.push_back(0);
return writePort(input);
}
void AtomicModel::write(std::ostream& out) const
{
out << "<model name=\"" << getName().c_str() << "\" "
<< "type=\"atomic\" ";
if (not conditions().empty()) {
out << "conditions=\"";
std::vector < std::string >::const_iterator it =
conditions().begin();
while (it != conditions().end()) {
out << it->c_str();
++it;
if (it != conditions().end()) {
out << ",";
}
}
out << "\" ";
}
out << "dynamics=\"" << dynamics().c_str() << "\" ";
if (not observables().empty()) {
out << "observables=\"" << observables().c_str() << "\" ";
}
writeGraphics(out);
out << ">\n";
writePort(out);
out << "</model>\n";
}
static int createEndpoints(int argc, char **argv)
{
cchar *endpoint;
char *ip;
int argind, port, secure;
ip = 0;
port = -1;
endpoint = 0;
argind = 0;
if ((app->appweb = maCreateAppweb()) == 0) {
mprLog("error appweb", 0, "Cannot create HTTP service");
return MPR_ERR_CANT_CREATE;
}
if ((app->server = maCreateServer(app->appweb, "default")) == 0) {
mprLog("error appweb", 0, "Cannot create HTTP server");
return MPR_ERR_CANT_CREATE;
}
loadStaticModules();
mprGC(MPR_GC_FORCE | MPR_GC_COMPLETE);
if (argind == argc) {
if (maParseConfig(app->server, app->configFile, 0) < 0) {
return MPR_ERR_CANT_CREATE;
}
} else {
app->documents = sclone(argv[argind++]);
if (argind == argc) {
if (maConfigureServer(app->server, NULL, app->home, app->documents, NULL, ME_HTTP_PORT, 0) < 0) {
return MPR_ERR_CANT_CREATE;
}
} else while (argind < argc) {
endpoint = argv[argind++];
mprParseSocketAddress(endpoint, &ip, &port, &secure, 80);
if (maConfigureServer(app->server, NULL, app->home, app->documents, ip, port, 0) < 0) {
return MPR_ERR_CANT_CREATE;
}
}
}
if (app->workers >= 0) {
mprSetMaxWorkers(app->workers);
}
/*
Call any ESP initializers from slink.c
*/
appwebStaticInitialize();
#if ME_WIN_LIKE
writePort(app->server);
#elif ME_UNIX_LIKE
addSignals();
#endif
mprGC(MPR_GC_FORCE | MPR_GC_COMPLETE);
return 0;
}
void Port::fullStop(){
vector<uint8_t > data;
data.push_back((uint8_t )115);
data.push_back((uint8_t )255);
data.push_back((uint8_t )255);
data.push_back((uint8_t )255);
data.push_back((uint8_t )255);
data.push_back((uint8_t )255);
writePort(data);
}
/**
* Auxiliary function for demo commands
* @param demoID is the ID of the requested demo
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::demoCode(int demoID) {
std::vector<char> input;
input.push_back(DEMO);
input.push_back(demoID);
return writePort(input);
}
void calibration(void)
{
/* Apply 2 pulses to TCK. */
writePort(g_ucPinTCK, 0x00);
writePort(g_ucPinTCK, 0x01);
writePort(g_ucPinTCK, 0x00);
writePort(g_ucPinTCK, 0x01);
writePort(g_ucPinTCK, 0x00);
ispVMDelay(0x8001);
/* Apply 2 pulses to TCK. */
writePort(g_ucPinTCK, 0x01);
writePort(g_ucPinTCK, 0x00);
writePort(g_ucPinTCK, 0x01);
writePort(g_ucPinTCK, 0x00);
}
/**
* This command causes Create to wait for
* the specified time. During this time,
* Create’s state does not change, nor
* does it react to any inputs, serial or
* otherwise.
* @param time an integer value specifies time
* to wait in tenths of a second with a resolution
* of 15 ms. Range: 0-255
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::waitTime(int time) {
std::vector<char> input;
if (time < TIME_MIN)
time = TIME_MIN;
else if (time > TIME_MAX)
time = TIME_MAX;
input.push_back(WAIT_TIME);
input.push_back((unsigned char) time);
return writePort(input);
}
/**
* This command causes Create to wait until
* it has rotated through specified angle in
* degrees. When Create turns counterclockwise,
* the angle is incremented. When Create turns
* clockwise, the angle is decremented. Until
* Create turns through the specified angle,
* its state does not change, nor does it
* react to any inputs, serial or otherwise.
* @param angle an integer specifies the angle
* Range: -32767 -32768
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::waitAngle(int angle) {
std::vector<char> input;
if (angle < ANGLE_MIN)
angle = ANGLE_MIN;
else if (angle > ANGLE_MAX)
angle = ANGLE_MAX;
input.push_back(WAIT_ANGLE);
input.push_back((short int) intToBytes(angle, 1));
input.push_back((short int) intToBytes(angle, 0));
return writePort(input);
}
/**
* This command causes iRobot Create to wait until
* it has traveled the specified distance in mm.
* When Create travels forward, the distance is
* incremented. When Create travels backward, the
* distance is decremented. If the wheels are
* passively rotated in either direction, the
* distance is incremented. Until Create travels
* the specified distance, its state does not
* change, nor does it react to any inputs,
* serial or otherwise.
* @param distance an integer specifies the distance
* Range: -32767 -32768
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::waitDistance(int distance) {
std::vector<char> input;
if (distance < DISTANCE_MIN)
distance = DISTANCE_MIN;
else if (distance > DISTANCE_MAX)
distance = DISTANCE_MAX;
input.push_back(WAIT_DISTANCE);
input.push_back(intToBytes((short int) distance, 1));
input.push_back(intToBytes((short int) distance, 0));
return writePort(input);
}
/**
* This command causes Create to wait until it detects
* the specified event. Until the specified event is
* detected, Create’s state does not change, nor does
* it react to any inputs, serial or otherwise.
* @param event an integer specifies the even id
* see Create OI for event IDs
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::waitEvent(int event) {
std::vector<char> input;
if (event < EVENT_WHEEL_DROP)
event = EVENT_NONE;
else if (event > EVENT_OI_MODE && event < EVENT_OI_MODE_NOT)
event = EVENT_NONE;
else if (event > EVENT_WHEEL_DROP_NOT)
event = EVENT_NONE;
input.push_back(WAIT_EVENT);
input.push_back((unsigned char) event);
return writePort(input);
}
void txPacket1(int port_num)
{
int idx;
uint8_t checksum = 0;
uint8_t total_packet_length = packetData[port_num].tx_packet[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
uint8_t written_packet_length = 0;
if (g_is_using[port_num])
{
packetData[port_num].communication_result = COMM_PORT_BUSY;
return ;
}
g_is_using[port_num] = True;
// check max packet length
if (total_packet_length > TXPACKET_MAX_LEN)
{
g_is_using[port_num] = False;
packetData[port_num].communication_result = COMM_TX_ERROR;
return;
}
// make packet header
packetData[port_num].tx_packet[PKT_HEADER0] = 0xFF;
packetData[port_num].tx_packet[PKT_HEADER1] = 0xFF;
// add a checksum to the packet
for (idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum
{
checksum += packetData[port_num].tx_packet[idx];
}
packetData[port_num].tx_packet[total_packet_length - 1] = ~checksum;
// tx packet
clearPort(port_num);
written_packet_length = writePort(port_num, packetData[port_num].tx_packet, total_packet_length);
if (total_packet_length != written_packet_length)
{
g_is_using[port_num] = False;
packetData[port_num].communication_result = COMM_TX_FAIL;
return;
}
packetData[port_num].communication_result = COMM_SUCCESS;
}
/*
* Initialize the Create
* @param mode the starting mode of Create
*/
void Create::initialize(Mode mode) {
/*
* When create is activated first it starts with
* Off mode. Once it receives the Start command,
* you can enter into any one of the four operating
* modes
*/
std::vector<char> input;
input.push_back(START);
if ( writePort(input) == EXIT_FAILURE ){
std::cerr<<"Create could not be started!"<<std::endl;
exit(1);
}
setMode(mode);
}
/**
* Sets the mode of create
* @param mode_ the mode for the create
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::setMode(Mode mode_) {
std::vector<char> input;
switch (mode_) {
/**
* When the OI is in Passive mode, you can request
* and receive sensor data using any of the sensors
* commands, but you cannot change the current
* command parameters for the actuators (motors,
* speaker, lights, low side drivers, digital outputs)
* to something else. To change how one of the
* actuators operates, you must switch from Passive
* mode to Full mode or Safe mode.
*/
case PASSIVE:
input.push_back(PASSIVE_MODE);
break;
/**
* Safe mode gives you full control of Create, with
* the exception of detection of a cliff, detection
* of wheel drop or charger plugin
*/
case SAFE:
input.push_back(SAFE_MODE);
break;
/**
* Full mode gives you complete control over Create,
* all of its actuators, and all of the safety-related
* conditions that are restricted when the OI is in
* Safe mode
*/
case FULL:
input.push_back(FULL_MODE);
break;
default:
break;
}
mode = mode_;
return writePort(input);
}
/**
* This command lets you control the forward and backward
* motion of Create’s drive wheels independently.
* @params rightVel an integer value which specifies the
* speed of right wheel. Range: -500-500
* @params leftVel an integer value which specifies the
* speed of left wheel. Range: -500-500
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::driveDirect(int rightVel, int leftVel) {
std::vector<char> input;
if (rightVel < VELOCITY_MIN)
rightVel = VELOCITY_MIN;
else if (rightVel > VELOCITY_MAX)
rightVel = VELOCITY_MAX;
if (leftVel < VELOCITY_MIN)
leftVel = VELOCITY_MIN;
else if (leftVel > VELOCITY_MAX)
leftVel = VELOCITY_MAX;
input.push_back(DRIVE_DIRECT);
input.push_back(intToBytes((short int) rightVel, 1));
input.push_back(intToBytes((short int) rightVel, 0));
input.push_back(intToBytes((short int) leftVel, 1));
input.push_back(intToBytes((short int) leftVel, 0));
return writePort(input);
}
/**
* This command controls the Create's drive wheels.
* @param speed an integer value which specifies the average
* speed of the drive wheels in millimeters per second (mm/s)
* Range: -500-500
* @param radius an integer value which specifies the
* radius in millimeters at which Create will turn. (higher radius
* makes movement straight) Range: -2000-2000
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::drive(int speed, int radius) {
std::vector<char> input;
if (speed < VELOCITY_MIN)
speed = VELOCITY_MIN;
else if (speed > VELOCITY_MAX)
speed = VELOCITY_MAX;
if (radius < RADIUS_MIN)
radius = RADIUS_MIN;
else if (radius > RADIUS_MAX)
radius = RADIUS_MAX;
input.push_back(DRIVE);
input.push_back(intToBytes((short int) speed, 1));
input.push_back(intToBytes((short int) speed, 0));
input.push_back(intToBytes((short int) radius, 1));
input.push_back(intToBytes((short int) radius, 0));
return writePort(input);
}
/*
* General command function which sends the given characters to port
* @param input an array of characters where each byte represents a byte
* @return EXIT_FAILURE or EXIT_SUCCESS
*/
int Create::command(std::vector<char> &input) {
return writePort(input);
}
MAIN(appweb, int argc, char **argv)
{
Mpr *mpr;
MaHttp *http;
cchar *ipAddrPort, *documentRoot, *argp, *logSpec;
char *configFile, *ipAddr, *homeDir, *timeText, *ejsPrefix, *ejsPath, *changeRoot;
int workers, outputVersion, argind, port;
documentRoot = 0;
changeRoot = ejsPrefix = ejsPath = 0;
ipAddrPort = 0;
ipAddr = 0;
port = -1;
logSpec = 0;
server = 0;
outputVersion = 0;
workers = -1;
configFile = BLD_FEATURE_CONFIG_FILE;
homeDir = BLD_FEATURE_SERVER_ROOT;
mpr = mprCreate(argc, argv, memoryFailure);
argc = mpr->argc;
argv = mpr->argv;
#if BLD_FEATURE_ROMFS
extern MprRomInode romFiles[];
mprSetRomFileSystem(mpr, romFiles);
#endif
if (osInit(mpr) < 0) {
exit(2);
}
if (mprStart(mpr, 0) < 0) {
mprUserError(mpr, "Can't start MPR for %s", mprGetAppName(mpr));
mprFree(mpr);
return MPR_ERR_CANT_INITIALIZE;
}
for (argind = 1; argind < argc; argind++) {
argp = argv[argind];
if (*argp != '-') {
break;
}
if (strcmp(argp, "--config") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
configFile = argv[++argind];
#if BLD_UNIX_LIKE
} else if (strcmp(argp, "--chroot") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
changeRoot = mprGetAbsPath(mpr, argv[++argind]);
#endif
} else if (strcmp(argp, "--debug") == 0 || strcmp(argp, "-D") == 0 ||
strcmp(argp, "-d") == 0 || strcmp(argp, "--debugger") == 0) {
mprSetDebugMode(mpr, 1);
} else if (strcmp(argp, "--ejs") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
ejsPrefix = mprStrdup(mpr, argv[++argind]);
if ((ejsPath = strchr(ejsPrefix, ':')) != 0) {
*ejsPath++ = '\0';
}
ejsPath = mprGetAbsPath(mpr, ejsPath);
} else if (strcmp(argp, "--home") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
homeDir = mprGetAbsPath(mpr, argv[++argind]);
if (chdir((char*) homeDir) < 0) {
mprPrintfError(mpr, "%s: Can't change directory to %s\n", mprGetAppName(mpr), homeDir);
exit(2);
}
} else if (strcmp(argp, "--log") == 0 || strcmp(argp, "-l") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
logSpec = argv[++argind];
maStartLogging(mpr, logSpec);
} else if (strcmp(argp, "--name") == 0 || strcmp(argp, "-n") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
mprSetAppName(mpr, argv[++argind], 0, 0);
} else if (strcmp(argp, "--threads") == 0) {
if (argind >= argc) {
return printUsage(mpr);
}
workers = atoi(argv[++argind]);
} else if (strcmp(argp, "--verbose") == 0 || strcmp(argp, "-v") == 0) {
maStartLogging(mpr, "stdout:2");
} else if (strcmp(argp, "--version") == 0 || strcmp(argp, "-V") == 0) {
outputVersion++;
} else {
mprPrintfError(mpr, "Unknown switch \"%s\"\n", argp);
printUsage(mpr);
exit(2);
}
}
if (argc > argind) {
if (argc > (argind + 2)) {
return printUsage(mpr);
}
ipAddrPort = argv[argind++];
if (argc > argind) {
documentRoot = argv[argind++];
} else {
documentRoot = ".";
}
}
if (outputVersion) {
mprPrintf(mpr, "%s %s-%s\n", mprGetAppTitle(mpr), BLD_VERSION, BLD_NUMBER);
exit(0);
}
if (ipAddrPort) {
mprParseIp(mpr, ipAddrPort, &ipAddr, &port, MA_SERVER_DEFAULT_PORT_NUM);
} else {
#if BLD_FEATURE_CONFIG_PARSE
if (configFile == 0) {
configFile = mprStrcat(mpr, -1, mprGetAppName(mpr), ".conf", NULL);
}
if (!mprPathExists(mpr, configFile, R_OK)) {
mprPrintfError(mpr, "Can't open config file %s\n", configFile);
exit(2);
}
#else
return printUsage(mpr);
#endif
#if !BLD_FEATURE_ROMFS
if (homeDir == 0) {
homeDir = mprGetPathParent(mpr, configFile);
if (chdir((char*) homeDir) < 0) {
mprPrintfError(mpr, "%s: Can't change directory to %s\n", mprGetAppName(mpr), homeDir);
exit(2);
}
}
#endif
}
homeDir = mprGetCurrentPath(mpr);
if (checkEnvironment(mpr, argv[0], homeDir) < 0) {
exit(3);
}
#if BLD_UNIX_LIKE
if (changeRoot) {
homeDir = mprGetAbsPath(mpr, changeRoot);
if (chdir(homeDir) < 0) {
mprError(mpr, "%s: Can't change directory to %s", homeDir);
exit(4);
}
if (chroot(homeDir) < 0) {
if (errno == EPERM) {
mprError(mpr, "%s: Must be super user to use the --chroot option", mprGetAppName(mpr));
} else {
mprError(mpr, "%s: Can't change change root directory to %s, errno %d",
mprGetAppName(mpr), homeDir, errno);
}
exit(5);
}
}
#endif
/*
* Create the top level HTTP service and default HTTP server. Set the initial server root to "."
*/
http = maCreateHttp(mpr);
if (http == 0) {
mprUserError(mpr, "Can't create HTTP service for %s", mprGetAppName(mpr));
return MPR_ERR_CANT_INITIALIZE;
}
server = maCreateServer(http, "default", ".", 0, -1);
if (server == 0) {
mprUserError(mpr, "Can't create HTTP server for %s", mprGetAppName(mpr));
return MPR_ERR_CANT_INITIALIZE;
}
if (maConfigureServer(mpr, http, server, configFile, ipAddr, port, documentRoot) < 0) {
/* mprUserError(mpr, "Can't configure the server, exiting."); */
exit(6);
}
if (mpr->ipAddr) {
mprLog(mpr, 2, "Server IP address %s", mpr->ipAddr);
}
timeText = mprFormatLocalTime(mpr, mprGetTime(mpr));
mprLog(mpr, 1, "Started at %s", timeText);
mprFree(timeText);
#if BLD_FEATURE_EJS
if (ejsPrefix) {
createEjsAlias(mpr, http, server, ejsPrefix, ejsPath);
}
#endif
#if BLD_FEATURE_MULTITHREAD
if (workers >= 0) {
mprSetMaxWorkers(http, workers);
}
#endif
#if BLD_WIN_LIKE
if (!ejsPrefix) {
writePort(server->defaultHost);
}
#endif
if (maStartHttp(http) < 0) {
mprUserError(mpr, "Can't start HTTP service, exiting.");
exit(7);
}
#if BLD_FEATURE_MULTITHREAD
mprLog(mpr, 1, "HTTP services are ready with max %d worker threads", mprGetMaxWorkers(mpr));
#else
mprLog(mpr, 1, "HTTP services are ready (single-threaded)");
#endif
/*
* Service I/O events until instructed to exit
*/
while (!mprIsExiting(mpr)) {
mprServiceEvents(mpr->dispatcher, -1, MPR_SERVICE_EVENTS | MPR_SERVICE_IO);
}
/*
* Signal a graceful shutdown
*/
mprLog(http, 1, "Exiting ...");
maStopHttp(http);
mprLog(http, 1, "Exit complete");
#if VXWORKS
if (mprStop(mpr)) {
mprFree(mpr);
}
#endif
return 0;
}
void Port::talkToSetare(int velocity, int angle, int rotation )
{
unsigned char A = 0 ;
float m1 , m2 , m3 , m4 ;
// int e1 = 255 - abs(m1);
// int e2 = 255 - abs(m2);
// float x = (1.0*e1)/(e1 + e2) ;
// m1 = 255*((-sin((45.0-angle)/180*3.14))*velocity / 100.0);
// m2 = 255*((-cos((45.0-angle)/180*3.14))*velocity / 100.0);
// m3 = -m1;
// m4 = -m2;
// m1 += (( x *rotation)/100.0)*255; m1 = floor(m1);
// m2 += (((1 - x)*rotation)/100.0)*255; m2 = floor(m2);
// m3 += (( x *rotation)/100.0)*255; m3 = floor(m3);
// m4 += (((1 - x)*rotation)/100.0)*255; m4 = floor(m4);
float vx, vy;
vx = sin((90.0-angle)/180*3.14);
vy = cos((90.0-angle)/180*3.14);
m1 = (vy-vx*sqrt(3)+rotation/100.0) / 3;
m2 = (-2*vy + rotation/100.0) / 3;
m3 = (vy+vx*sqrt(3)+rotation/100.0) / 3;
float e = max(abs(m1*1000), max(abs(m2*1000), abs(m3*1000))) / 1000.0;
m1 = (velocity/100.0)*((m1/e) * 255);
m2 = (velocity/100.0)*((m2/e) * 255);
m3 = (velocity/100.0)*((m3/e) * 255);
m4 = 0;
if (velocity == 0) {
m1 = (rotation/100.0)*255;
m2 = (rotation/100.0)*255;
m3 = (rotation/100.0)*255;
}
if(m1>0)
A|=1<<0;
else if(m1<0)
A|=1<<1;
if(m2>0)
A|=1<<2;
else if(m2<0)
A|=1<<3;
if(m3>0)
A|=1<<4;
else if(m3<0)
A|=1<<5;
if(m4>0)
A|=1<<6;
else if(m4<0)
A|=1<<7;
vector<uint8_t> data;
data.push_back((uint8_t)115);
data.push_back((uint8_t)abs(m1));
data.push_back((uint8_t)abs(m2));
data.push_back((uint8_t)abs(m3));
data.push_back((uint8_t)abs(m4));
data.push_back((uint8_t)A);
writePort(data);
return;
}
void CWriterThread::run(){
writePort();
return;
}
void sclock(void)
{
writePort(g_ucPinTCK, 0x01);
writePort(g_ucPinTCK, 0x00);
}
