void setTask(void) {
if (baseSpeed != 0) {
for (uint8_t i = 0; i < 2; i++) {
int16_t diff = pidOutput[i];
// Base-Speed is always positive, diff could be negative
if (diff > 0) {
if (diff > baseSpeed) {
diff = baseSpeed; // Limit PID
}
} else {
if (diff < -baseSpeed) {
diff = -baseSpeed; // Limit PID
}
}
uint8_t v[2] = { baseSpeed + diff, baseSpeed - diff };
if (v[0] < 10)
v[0] = 10; // Keep Motors running
if (v[1] < 10)
v[1] = 10;
setMotorSpeeds(i, v);
}
} else {
// Motors stopped
uint8_t v[2] = {0, 0};
setMotorSpeeds(0, v);
setMotorSpeeds(1, v);
}
}
void Forward(float distance)
{
int i = (distance < 0) ? -1 : 1;
setMotorSpeeds(i*FORWARD_SPEED, i*FORWARD_SPEED);
writeDebugStreamLine("FORWARD: %f", distance);
int timeLeft = (int) (i*distance*FORWARD_CONST/(float)FORWARD_SPEED);
wait1Msec(timeLeft);
setMotorSpeeds(0, 0);
}
/*
* +ve angles are anticlockwise, angle in degrees
*/
void Turn(float a)
{
int i = (a < 0) ? -1 : 1;
setMotorSpeeds(-i*TURN_SPEED, i*TURN_SPEED);
writeDebugStreamLine("TURN: %f", a);
// Correction for motor bias
wait1Msec(i*degToRad(a)*(float)TURN_CONST/(float)TURN_SPEED);
setMotorSpeeds(0, 0);
}
int main()
{
MemController<CarState> ctrl = MemController<CarState>();
CarState state;
//initial state:
state = ctrl.getLastElement();
memset(&state,0,sizeof(state));
ctrl.pushElement(state);
while(1)
{
for(int i=0;i<5000;i++){;}
int nios;
// get the lastest car state from the shared memory
state = ctrl.getLastElement(true);
// print some diagnostics information
int speed = state.motorEcus[0].iCurrentSpeed + state.motorEcus[1].iCurrentSpeed + state.motorEcus[2].iCurrentSpeed + state.motorEcus[3].iCurrentSpeed;
LOG_DEBUG("\rSpeed: %+5d mm/s, OpMode: %#x ", speed, state.currMode);
//LOG_DEBUG("yippee!");
// perform state switch if requested.
if(state.reqMode != state.currMode)
{
switchState(&state);
}
state.counterCarControl=state.counterComm;
setMotorSpeeds(&state);
if(state.currMode==OPMODE_MANUDRIVE){
state.ip1=state.reqip1;
state.ip2=state.reqip2;
state.ip3=state.reqip3;
state.ip4=state.reqip4;
}
else{
state.ip1=VCIPPart1;
state.ip2=VCIPPart2;
state.ip3=VCIPPart3;
state.ip4=VCIPPart4;
}
ctrl.pushElement(state);
// TODO: write a delay function w/ timer. Otherwise we might run into problems blocking the mutex from all the shared memory reads...
//delay(10);
for (int i = 0; i < 10000; i++) {;}
}
return -1;
}
int main(int argc, char **argv)
{
printf("**** MD22 test application ****\n");
if ((fd = open(fileName, O_RDWR)) < 0) { // Open port for reading and writing
printf("Failed to open i2c port\n");
exit(1);
}
if (ioctl(fd, I2C_SLAVE, address) < 0) { // Set the port options and set the address of the device we wish to speak to
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 7; // This is the register we wish to read software version from
if ((write(fd, buf, 1)) != 1) { // Send regiter to read from
printf("Error writing to i2c slave\n");
exit(1);
}
if (read(fd, buf, 1) != 1) { // Read back data into buf[]
printf("Unable to read from slave\n");
exit(1);
}
else {
printf("Software v: %u \n",buf[0]);
}
setAcceleration(); // Set acceleration to max this will give us the longest time to reach a set speed
setMotorSpeeds(255); // speed to full forward
sleep(5); // Pause to allow MD22 to reach speed
setMotorSpeeds(0); // Full reverse
sleep(10);
setMotorSpeeds(128); // Stop
sleep(5);
return 0;
}
void devicePwmMotorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_MOVE_MOTOR) {
signed int left = readSignedHex2(inputStream);
signed int right = readSignedHex2(inputStream);
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_MOVE_MOTOR);
setMotorSpeeds(left * 2, right * 2);
}
else if (commandHeader == COMMAND_READ_MOTOR_VALUE) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_READ_MOTOR_VALUE);
signed int left = getLeftSpeed();
signed int right = getRightSpeed();
appendHex2(outputStream, left / 2);
appendHex2(outputStream, right / 2);
}
else if (commandHeader == COMMAND_STOP_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_STOP_MOTOR);
stopMotors();
}
else if (commandHeader == COMMAND_TEST_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_TEST_MOTOR);
appendString(getAlwaysOutputStreamLogger(), "Left Forward\n");
// Left forward
setMotorSpeeds(50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right forward
setMotorSpeeds(0, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Left Backward\n");
// Left backward
setMotorSpeeds(-50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right backward
setMotorSpeeds(0, -50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Forward\n");
// Both forward
setMotorSpeeds(50, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Backward\n");
// Both backward
setMotorSpeeds(-50, -50);
delaymSec(2000);
stopMotors();
}
}
/*
* Follows gap until gap found
*/
void ForwardWallFollow(bool sonarFacingLeft)
{
writeDebugStreamLine("WALL_FOLLOW");
int var;
float sensorDist;
float prevSensorDist = -1;
int count = 0, count2 = 0, switchCount = 0;
int lspeed, rspeed;
bool switchL, switchR;
//int timeLeft = (int) (distance*FORWARD_CONST/(float)FORWARD_SPEED);
while (true)
{
sensorDist = SensorValue[sonarSensor];
if (sensorDist > SENSOR_MAX) continue; // Ignore error readings
if (switchL || switchR)
switchCount++;
if (prevSensorDist == sensorDist) {
count2++;
if (count2 > 25) {
if (!sonarFacingLeft && abs(lspeed) > FORWARD_SPEED) {
switchR = true;
switchL = false;
}
else if (sonarFacingLeft && abs(rspeed) > FORWARD_SPEED) {
switchR = false;
switchL = true;
}
count2 = 0;
}
}
else count2 = 0;
// If difference between current and previous sensor reading is large, we have found a gap
if (abs(prevSensorDist - sensorDist) > 15 && prevSensorDist != -1)
count++; // Possibly found gap
else {
count = 0;
prevSensorDist = sensorDist;
}
if (count > 7) // && abs(var) < 10)
break; // Almost definately found gap
if (switchCount > 25)
switchR = false;
switchL = false;
}
var = WALL_FOLLOW_K*(sensorDist - DESIRED_WALL_DIST);
if (sonarFacingLeft || switchL) {
lspeed = FORWARD_SPEED-var;
rspeed = FORWARD_SPEED+var;
}
else if (!sonarFacingLeft || switchR) {
lspeed = FORWARD_SPEED+var;
rspeed = FORWARD_SPEED-var;
}
setMotorSpeeds(lspeed, rspeed);
writeDebugStreamLine("MOTORS: %d %f", var, sensorDist);
//wait1Msec(1);
setMotorSpeeds(0, 0);
PlayImmediateTone(500, 30);
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
resetPID();
}
#endif
if (millis() > nextTick) {
ticks += 1;
nextTick += TICK_INTERVAL;
}
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_TICKS:
Serial.println(ticks);
break;
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
Serial.println("OK");
break;
case READ_MOTORS:
Serial.print(leftPID.output);
Serial.print(" ");
Serial.println(rightPID.output);
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
resetPID();
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
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);
}
}
}
