void iC_PWMLowSide(uint8 LSD2Duty, uint8 LSD1Duty, uint8 LSD0Duty)
{
sendSerial(iCreateCom,144);
sendSerial(iCreateCom,LSD2Duty);
sendSerial(iCreateCom,LSD1Duty);
sendSerial(iCreateCom,LSD0Duty);
}
void iC_LEDs(uint8 LEDBits, uint8 powerCol, uint8 powerInt)
{
sendSerial(iCreateCom,139);
sendSerial(iCreateCom,LEDBits);
sendSerial(iCreateCom,powerCol);
sendSerial(iCreateCom,powerInt);
}
double depthProbe::getRealZero(double X, double Y,bool reset)
{
port.flush(port_nr);
double Zpos=.5;
QString comando;
QString received = "";
comando = "G00 X";
comando.append(QString::number(X)).append(" Y").append(QString::number(Y)).append("\r");
sendSerial(comando);
port.flush(port_nr);
pause(50);
while(!received.contains("depth"))
{
Zpos-=0.02;
comando = "G00 Z";
comando.append(QString::number(Zpos)).append("\r");
sendSerial(comando);
pause(50);
received = readSerial(false);
}
if(reset)
port.Reset(port_nr);
sendSerial("G00 Z01\r");
pause(2000);
return(Zpos);
}
void iC_DriveDirect(uint8 rVelocity_h, uint8 rVelocity_l, uint8 lVelocity_h, uint8 lVelocity_l)
{
sendSerial(iCreateCom,145);
sendSerial(iCreateCom,rVelocity_h);
sendSerial(iCreateCom,rVelocity_l);
sendSerial(iCreateCom,lVelocity_h);
sendSerial(iCreateCom,lVelocity_l);
}
void iC_Drive(uint8 velocity_h, uint8 velocity_l, uint8 radius_h, uint8 radius_l)
{
sendSerial(iCreateCom,137);
sendSerial(iCreateCom,velocity_h);
sendSerial(iCreateCom,velocity_l);
sendSerial(iCreateCom,radius_h);
sendSerial(iCreateCom,radius_l);
}
void depthProbe::button()
{
readSerial(true);
sendSerial("$\r");
QString popochas = readSerial(false);
QMessageBox(QMessageBox::Critical,"Interaction needed",popochas,QMessageBox::Ok).exec();
}
/*JSON{
"type" : "idle",
"generate" : "jswrap_wice_idle"
}*/
bool jswrap_wice_idle() {
#ifdef GATEWAY
sendSerial();
if (restartWifiAt != 0 && restartWifiAt < jshGetSystemTime()) {
blink(PIN_GRN, 2000);
disableWifi();
blink(PIN_RED, 4000);
enableWifi();
restartWifiAt = 0;
startTime = jshGetSystemTime() + jshGetTimeFromMilliseconds(5000);
}
#else //NODE
if (hasConnection && hasConnection == needConnection){
blink(PIN_BLUE, 20);
blink(PIN_BLUE, 20);
needConnection = false;
disableDASH();
doTimeout = true; //start timing out again
}
if (doTimeout && transmittingData && !jshHasTransmitData()) {
transmittingData = false;
delay(20); //DO NOT BLOCK
disableDASH();
}
#endif
return false;
}
//--------------------------------------------------------------
void Mission1::update()
{
if(start) {
p.y -= 30;
}
// each 1 second
if (ofGetElapsedTimeMillis() - getSharedData().lastUpdate > 1000)
{
getSharedData().counter++;
getSharedData().lastUpdate = ofGetElapsedTimeMillis();
// 時間がたったら次へ
//if (getSharedData().counter > 21) {
if (getSharedData().counter > 10) {
sendSerial(0); // 0の信号を送る
// initialize
x = 0;
xx = 0;
p.set(ofGetWidth()/2-90, 1920);
changeState("Mission2");
}
}
}
int main(int argc, char *argv[])
// arg1: serial device file
// arg2: optional timeout in seconds, default 60
// arg3: optional 'nolog' to carry on when filesystem full
{
int commfd = 0;
int option; // command line processing
int baud = 9600;
char * cp;
char table[128];
int flags = CS8 | CLOCAL | CREAD;
char * serialName = SERIALNAME;
memset(table, 0, 128);
// Command line arguments
init_table(table);
opterr = 0;
while ((option = getopt(argc, argv, "b:dVcs:?")) != -1) {
DEBUG fprintf(stderr, "Option %c ", option);
switch (option) {
case 'b': baud = atoi(optarg); break;
case 'c': flags |= CRTSCTS; break;
case '?': usage(); exit(1);
case 's': serialName = optarg; break;
case 'd': debug++; break;
case 'V': printf("Version: %s\n", VERSION); exit(0);
}
}
// Open serial port
#ifdef DEBUGCOMMS
commfd = 0;
#else
if ((commfd = openSerial(serialName, baud, 0, flags, 1)) < 0) {
fprintf(stderr, "ERROR 7seg Failed to open %s at %d: %s", serialName, baud, strerror(errno));
exit(1);
}
#endif
DEBUG2 fprintf(stderr, "Serial port %s open ...", serialName);
// Send data
while (optind < argc) { // remainder of command line must be bytes
for (cp = argv[optind]; *cp; cp++) {
DEBUG fprintf(stderr, "'%c' [%02x] ", *cp, table[*cp]);
sendSerial(commfd, table[*cp]);
}
optind++;
}
DEBUG fprintf(stderr, "\n");
closeSerial(commfd);
return 0;
}
/*
* Process the serial data to check for incoming commands.
* A Response code is returned depending on the command processed.
*/
int BLEModule::processSerial()
{
while (Serial.available())
{
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
if(!setupMode)
{
//If the incoming character is a newline
//Then the string is complete.
//##The teminating char is not added to the string##
if (inChar == '\n')
{
bleStringComplete = true;
}
else
{
bleInputString += inChar;
}
}
}
int status = 0;
if(bleStringComplete)
{
if(bleInputString.equals("+"))
{
status = 2;
}
else if(bleInputString.equals("-"))
{
status = 3;
}
else if(bleInputString.equals("*"))
{
status = 4;
}
else if(bleInputString.equals("$!;"))
{
status = 5;
}
else
{
//No conditions were met.
//Send the incoming data back for debugging.
sendSerial(bleInputString);
status = 1;
}
bleInputString = "";
bleStringComplete = false;
}
//return the status of the serial data.
return status;
}
inline void
hd44780::cmd( uint8_t data ) {
// Set RS value to low
m_mode->write( 0, 0 );
// Send the command to the display
sendSerial( data, 5000 );
}
void depthProbe::getZero()
{
port.flush(port_nr);
double Zpos=0;
QString comando,num;
QString received = "";
while(!received.contains("depth"))
{
comando="G00 Z";
Zpos-=0.02;
comando.append(num.setNum(Zpos)).append("\r");
sendSerial(comando);
pause(50);
received = readSerial(false);
}
port.Reset(port_nr);
sendSerial("G00 Z01\r");
pause(100);
}
void depthProbe::Run()
{
if(!zeroed)
Zero();
double realZero;
double X,Y,Z;
int orig;
QString line = "";
QFile file(ui->filePath->text());
file.open(QFile::ReadOnly);
QTextStream code(&file);
while(!code.atEnd())
{
commands.append(code.readLine());
}
file.close();
commands.removeAll("");
origSize = commands.size();
orig = commands.size();
file.open(QFile::WriteOnly);
QTextStream write(&file);
while(!commands.empty())
{
line = commands.first();
if((line.toUpper().contains("X"))&&(line.toUpper().contains("Y")))
{
QString temp = line.mid(line.toUpper().indexOf("X"));
X = temp.mid(1,temp.indexOf(" ")).toDouble();
temp = temp.mid(temp.toUpper().indexOf("Y"));
Y = temp.mid(1,temp.indexOf(" ")).toDouble();
}
if(line.toUpper().contains("Z"))
{
if(line.lastIndexOf(" ")>line.toUpper().indexOf("Z"))
{
QString temp = line.mid(line.toUpper().indexOf("Z")+1);
Z=temp.mid(0,temp.indexOf(" ")).toDouble();
}
else
Z=line.mid(line.toUpper().indexOf("Z")+1).toDouble();
if(Z<0)//edit depth
{
realZero = getRealZero(X,Y,false);
line.replace(QString::number(Z),QString::number(Z+realZero));
}
}
write.operator <<(line);
write.operator <<("\r\n");
commands.removeFirst();
updateBar(double(commands.size()),double(orig));
}
file.close();
sendSerial("G00 X0 Y0 Z0\r");
pause(50);
}
/*
* Initial module setup.
* Starts the module after setup is complete.
*/
void BLEModule::initialSetup()
{
//Full initialisation of the module from scratch.
if(firstTimeSetup)
{
sendSerial(SET_ROLE_SLAVE);
delay(300);
sendSerial(SET_MODULE_NAME_SQUASHT);
delay(300);
sendSerial(RESET_MODULE);
delay(500);
}
//Start the module.
sendSerial(START_MODULE);
delay(500);
flushSerial(); //Needed to clear out any leftover responses.
setupMode=false;
}
double depthProbe::depthProbeZero()
{
double half=2.56,level = 0;
int i;
QString received;
sendSerial(QString("G00 Z").append(QString::number(level)).append("\r"));
for(i=0;i<8;i++)
{
received=readSerial(true);
half=half/2;
if(received.contains("depth"))
{
level+=half;
}
else
{
level-=half;
}
sendSerial(QString("G00 Z").append(QString::number(level)).append("\r"));
}
return level;
}
/*
* Setup and start the Serial connection to the BLE module.
*/
void BLEModule::beginSerial()
{
//If the module is being setup for the first time
//then we need to set the correct buadrate.
if(firstTimeSetup)
{
Serial.begin(9600);
delay(500);
sendSerial(SET_BAUD);
delay(500);
}
//Set our module to the correct baud rate.
Serial.begin(__BAUD_RATE__);
delay(500);
}
void iC_DigitalOut(uint8 outBits)
{
sendSerial(iCreateCom,147);
sendSerial(iCreateCom,outBits);
}
int main()
{
int cTotal = 0;
int cState = STATE_READ;
char toggleOut = FALSE;
Signal motor1;
Signal motor2;
Signal estop;
Signal rcMode;
init_coridium();
setbaud(0, 17);
statusMessage[0] = '#';
statusMessage[1] = '%';
statusMessage[2] = 0x82;
INPUT(RC1); motor1.pin = RC1;
INPUT(RC2); motor2.pin = RC2;
INPUT(RC3); estop.pin = RC3;
INPUT(RC4); rcMode.pin = RC4;
OUTPUT(TX);
OUTPUT(ESTOP); LOW(ESTOP);
OUTPUT(RCMode);
OUTPUT(FreqPin);
OUTPUT(LIGHT);
motor1.state = STATE_WAIT;
motor1.duty = 0;
motor1.valid = 0;
motor2.state = STATE_WAIT;
motor2.duty = 0;
motor2.valid = 0;
estop.state = STATE_WAIT;
estop.duty = 0;
estop.valid = 0;
rcMode.state = STATE_WAIT;
rcMode.duty = 0;
rcMode.valid = 0;
SLEEP(2);
while (TRUE)
{
switch (cState)
{
case STATE_READ:
if (toggleOut)
{
toggleOut = FALSE;
HIGH(FreqPin);
}
else
{
toggleOut = TRUE;
LOW(FreqPin);
}
cTotal += getDuty(&motor1);
cTotal += getDuty(&motor2);
cTotal += getDuty(&estop);
cTotal += getDuty(&rcMode);
if (cTotal > 8)
{
cState = STATE_SERIAL;
cTotal = 0;
}
break;
case STATE_SERIAL:
if (estop.lastVal > 0)
{
sendESTOP(0);
gESTOP = FALSE;
if (rcMode.lastVal > 0)
{
HIGH(LIGHT);
HIGH(RCMode);
// 11 == Manual (teleop) Mode
statusMessage[3] = 0x00;
statusMessage[4] = 0xFF;
statusMessage[5] = 0xFF;
statusMessage[6] = 0xFF;
calcSum();
//statusMessage[7] = 0x84;
//SEROUT(TXD0,9600, 1, 3, statusMessage);
TXD0(statusMessage[0]);
TXD0(statusMessage[1]);
TXD0(statusMessage[2]);
TXD0(statusMessage[3]);
TXD0(statusMessage[4]);
TXD0(statusMessage[5]);
TXD0(statusMessage[6]);
TXD0(statusMessage[7]);
}
else
{
if (flashcount > 0)
{
HIGH(LIGHT);
}
else
{
LOW(LIGHT);
}
if (flashcount > LIGHTDUTY)
{
flashcount = -LIGHTDUTY;
}
LOW(RCMode);
// 00 == Autonomous Mode
statusMessage[3] = 0x00;
statusMessage[4] = 0x00;
statusMessage[5] = 0x00;
statusMessage[6] = 0x00;
calcSum();
//statusMessage[7] = 0x84;
//SEROUT(TXD0,9600, 1, 3, statusMessage);
TXD0(statusMessage[0]);
TXD0(statusMessage[1]);
TXD0(statusMessage[2]);
TXD0(statusMessage[3]);
TXD0(statusMessage[4]);
TXD0(statusMessage[5]);
TXD0(statusMessage[6]);
TXD0(statusMessage[7]);
}
}
else
{
HIGH(LIGHT);
HIGH(RCMode);
sendESTOP(1);
gESTOP = TRUE;
// 11 == Manual (teleop) Mode
statusMessage[3] = 0x00;
statusMessage[4] = 0xFF;
statusMessage[5] = 0xFF;
statusMessage[6] = 0xFF;
calcSum();
//statusMessage[7] = 0x84;
//SEROUT(TXD0,9600, 1, 3, statusMessage);
TXD0(statusMessage[0]);
TXD0(statusMessage[1]);
TXD0(statusMessage[2]);
TXD0(statusMessage[3]);
TXD0(statusMessage[4]);
TXD0(statusMessage[5]);
TXD0(statusMessage[6]);
TXD0(statusMessage[7]);
}
#ifdef DEBUG
putchar( binToHexstr((char)((motor1.lastVal >> 4) & 0x0F)) );
putchar( binToHexstr((char)(motor1.lastVal & 0x0F)) );
putchar(' ');
putchar( binToHexstr((char)((motor2.lastVal >> 4) & 0x0F)) );
putchar( binToHexstr((char)(motor2.lastVal & 0x0F)) );
putchar('\n');
//printf("%d\n", estop.lastVal);
#endif
// motor1 == forward value
// motor2 == left/right value
int left_motor = motor1.lastVal/2 - motor2.lastVal/2;
int right_motor = motor1.lastVal/2 + motor2.lastVal/2;
sendSerial(right_motor, 1);
sendSerial(left_motor, 0);
cState = STATE_READ;
break;
}
flashcount++;
}
return 0;
}
void iC_Sensor(uint8 sensorId)
{
sendSerial(iCreateCom,142);
sendSerial(iCreateCom,sensorId);
}
//primitive d'initialisation de la comunication avec l'iCreate
void iC_Start(void)
{
sendSerial(iCreateCom,128);
flushRecieveBuffer(iCreateCom);
}
void iC_Baud(uint8 baudCode)
{
/*rajouter un test de validité de la valeur*/
sendSerial(iCreateCom,129);
sendSerial(iCreateCom,baudCode);
}
void iC_Safe(void)
{
sendSerial(iCreateCom,131);
}
void iC_Full(void)
{
sendSerial(iCreateCom,132);
}
void iC_SendIR(uint8 byteValue)
{
sendSerial(iCreateCom,151);
sendSerial(iCreateCom,byteValue);
}
void iC_LowSideDrivers(uint8 driverBits)
{
sendSerial(iCreateCom,138);
sendSerial(iCreateCom,driverBits);
}
void iC_Demo(uint8 demoID)
{
sendSerial(iCreateCom,136);
sendSerial(iCreateCom,demoID);
}
