int main()
{
//simpleterm_close(); //close default terminal, I want to use those pins
//if the default uart (Universal Asynchronous Recieve and Transmitting
// pins are not being used, this isn't needed
startComs(RX_PIN, TX_PIN, BAUD, 1000); //this will go to the bluetooth module eventually
// startSensor(PING_PIN, LEFT_WHISKER, RIGHT_WHISKER);
int speed = 20; //intial speed 20 ticks/s
int n = 0;
while(1) //repeat until power loss
{
int command = rxCommand();
switch(command) //wait for a control byte
{
case 'e':
txInt32(n); //sending incrementing numbers
n+=10000;
break;
case 'b':
txInt32(rxInt32()*2); //double received number
break;
case '?': //send 42
txInt32(42);
break;
case '0':
startSensor(PING_PIN, LEFT_WHISKER, RIGHT_WHISKER);
break;
case 'z': //run function "startWander"
startWander();
break;
case 'x': //run function "stopWander"
stopWander();
drive_speed(0,0);
break;
case 'f': //send sensor data
print("p %d\tpc %d\tpi %d\twl %d\twr %d\ttl %d\ttr %d\n",getPing(), getPingcm(), getPingin(), getWhiskerL(), getWhiskerR(), getTicksL(), getTicksR());
break;
case 'v':
txInt32(getTicksL());
txInt32(getTicksR());
txInt32(getPing());
txInt32(getWhiskerL());
txInt32(getWhiskerR());
break;
case 'h': //recieve drive speed from computer
drive_speed(rxInt32(),rxInt32());
break;
case 'p':
__builtin_propeller_clkset(0x80); //reboot
break;
case 'q': //stop movement
drive_speed(0,0);
break;
case 'w': //move forward
drive_speed(speed,speed);
break;
case 'a': //pivot left
drive_speed(-speed,speed);
break;
case 's': //move backward
drive_speed(-speed,-speed);
break;
case 'd': //pivot right
drive_speed(speed,-speed);
break;
case 't': //toggle turbo mode up to 70 ticks/s
if (speed>50) speed = 20; else speed =70;
break;
case 'j': //computer input for rotation
drive_goto(rxInt32(),rxInt32());
break;
case 'i': // print
print("X = %.2f, Y = %.2f, Th = %.1f\n", (double) getXcm(), (double) getYcm(), (double) getThdeg());
break;
case 'k': //kalibrate
while(getThdeg()<90)
{
drive_speed(-speed,speed);
}
drive_speed(0,0);
break;
case -1: //case when timed out
txInt32(314);
break;
default: //unknown command
txInt32(2718);
txInt32(command);
//print(command);
print("\n%d\n",command);
}
//Note: The computer can sometimes miss bytes
//when the bytes are coming really fast
}
}
bool ProximityAdaptorEvdev::resume()
{
startSensor();
return true;
}
void runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}