![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test
Dependencies: MODDMA MODSERIAL mbed
Fork of IRIS_MBED by
main.cpp
- Committer:
- JonathanAshworth
- Date:
- 2015-04-09
- Revision:
- 6:8ffc6d3a7c1d
- Parent:
- 5:d0b2b4d8b9ba
File content as of revision 6:8ffc6d3a7c1d:
/************************************************************************************ PROJECT IRIS Multidisciplinary Group Project ENG701S2 (Year 4 - MENG) at the University of Portsmouth Filename: main.cpp Description: Root source file for the project. The protocol's timeout functionality is achieved using timer3 therefore the use of wait() or similar must be avoided else the system will not adhere to the timeout timings (wait and timer both use timer3). Original Created: 05/04/2015 Original Author: Jonathan Ashworth --------------------------Change Log-------------------------------- Date | Author | Details 06/04/05 Jonathan Ashworth ************************************************************************************/ #include "main.h" //************************ For Development Only ************************************* #define DEBUG 0 // When set GGPSSerial becomes a debug output #define DUMMY 1 // When set botData is filled with random data #define PC_SERIAL_INIT_DISABLE 0 DigitalOut led1(LED1); // Timeout led - idle DigitalOut led2(LED2); // rx tacking too long timeout DigitalOut led3(LED3); // bad write timeout DigitalOut led4(LED4); // Invalid value requested to servo/motor - construct tx - data too soon //*********************************************************************************** //************************** Serial DMA Setup *************************************** MODSERIAL PCSerial( PC_TX_PIN, // MBED's transmit pin to PC PC_RX_PIN, // MBED's receive pin from PC PC_TX_BUFFER_SIZE, // Transmiter buffer size PC_RX_BUFFER_SIZE); // Receiver buffer size MODSERIAL SSC32Serial(SSC32_TX_PIN, // MBED's transmit pin to SSC32 SSC32_RX_PIN, // MBED's receive pin from SSC32 SSC32_TX_BUFFER_SIZE, // Transmiter buffer size SSC32_RX_BUFFER_SIZE); // Receiver buffer size MODSERIAL GPSSerial(GPS_TX_PIN, // MBED's transmit pin to GPS GPS_RX_PIN, // MBED's receive pin from GPS GPS_TX_BUFFER_SIZE, // Transmiter buffer size GPS_RX_BUFFER_SIZE); // Receiver buffer size MODDMA dma; // Define DMA hardware for MODSERIAL to use // Storage of whole PC requests (termination detected): char PCRxData[PC_RX_BUFFER_SIZE] = {0}; // Storage of whole PC responses: char PCTxData[PC_TX_BUFFER_SIZE] = {0}; // Temporary storage of incomming PC bytes as they arrive: char incommingPCRxData[PC_RX_BUFFER_SIZE] = {0}; // Tracking the length of the incomming PC data (number of bytes received): char incommingPCRxDataCount = 0; // Storage for requested servo position (set by SSC32's rx ISR): char servoPositionReturn = 0; // PC serial comms timout timer: Timer PCTimeoutTimer; //*********************************************************************************** //************************** ESC Drive Setup **************************************** PwmOut driveNS(DRIVE_NS_PIN); // Nearside ESC PwmOut driveOS(DRIVE_OS_PIN); // Offside ESC //*********************************************************************************** //HCSR04 usound(p17, p18); //trig echo Flags flags; // Event/status tracking flag structure botStateHandler botData; // Storage class for all peripherals // limit pitch 1300 lower limit 2200 max int main(void) { initialise(); while(!flags.PCBegin); // Wait for the PC to send the begin phrase while(1) { // If the PC serial rx line is idle for too long then timeout: if (PCTimeoutTimer.read_ms() > PC_TIMEOUT_PERIOD) { flags.PCTimeout = 1; led1 = 1; if(DEBUG) { GPSSerial.printf("Idle timeout\r\n"); } } // If a timeout has been set anywhere then process the timeout function: if (flags.PCTimeout) { PCTimeout(); } if (flags.rxNewData) { // If a new data packet has been received: parseRequestData(); // Complete the requests flags.rxNewData = 0; // Data is now old so reset flag } if (flags.txNewData) { sendPCSerialData(); // Return the requested data flags.txNewData = 0; // Data is now old so reset flag } } } void initialise(void) { // Setup serial links and their ISRs: PCSerial.baud(PC_BAUDRATE); PCSerial.attach(&dmaPCSerialRx, MODSERIAL::RxIrq); // rx ISR SSC32Serial.baud(SSC32_BAUDRATE); SSC32Serial.attach(&dmaSSC32SerialRx, MODSERIAL::RxIrq); // rx ISR GPSSerial.baud(GPS_BAUDRATE); GPSSerial.attach(&dmaGPSSerialRx, MODSERIAL::RxIrq); // rx ISR // Stop and reset the timeout timer: PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); // Define the PWM period for the drive ESCs: driveNS.period_ms(20); // Nearside driveOS.period_ms(20); // Offside // Set the channels for each servo in field 0 of the bot's DB: float armBaseChannel = SERVO_ARM_BASE_CHAN; float armShoulderChannel = SERVO_ARM_SHOULDER_CHAN; float armElbowChannel = SERVO_ARM_ELBOW_CHAN; float armWristChannel = SERVO_ARM_WRIST_CHAN; float armManChannel = SERVO_ARM_MANIPULATOR_CHAN; float visionPitchChannel = SERVO_VISION_PITCH_CHAN; float visionYawChannel = SERVO_VISION_YAW_CHAN; botData.setVal(SID_ARM_BASE, 0, &armBaseChannel); botData.setVal(SID_ARM_SHOULDER, 0, &armShoulderChannel); botData.setVal(SID_ARM_ELBOW, 0, &armElbowChannel); botData.setVal(SID_ARM_WRIST, 0, &armWristChannel); botData.setVal(SID_ARM_MANIPULATOR, 0, &armManChannel); botData.setVal(SID_VISION_PITCH, 0, &visionPitchChannel); botData.setVal(SID_VISION_YAW, 0, &visionYawChannel); shutdown(); // Ensure the bot is in a safe shutdown mode/position if (DEBUG) { GPSSerial.printf("Hello\r\n"); // Aid setup of a terminal to the MBED } flags.PCBegin = (bool)PC_SERIAL_INIT_DISABLE; // TODO: remove } /* void updateSensorsData(void) { int pirAlarm = mbed_PIR(); float temperature = mbed_tempsensor(); float humidity = mbed_humiditysensor(); float compassX = getcompassX(); float compassY = getcompassY(); float compassZ = getcompassZ(); float gyroX = getgyroX(); float gyroY = getgyroY(); float gyroZ = getgyroZ(); int accelX = getaccelX(); int accelY = getaccelY(); int accelZ = getaccelZ(); } */ /* void getServoData(Servo *servo) { SSC32Serial.printf("QP %d\r", servo->channel); int temp = 1; //read reply //convert char to int servo->position = temp; } */ long readUltrasound(void) {/* //Set Mux to sensor->muxChannel //Pulse trigger pin //Disable interrupts //Read PWM on echo pin //Enable interrupts //sensor->distanceMillimeters = result;*/ long distance = 0; //distance = usound.distance(1); return distance; } void constructTxData(char *SIDs, char numberOfSensors) { // TODO: Tidy up int i, j, lengthOfCurrentSentence; char sendSentence[PC_LONGESTRESPONSESENTENCE] = {0}; char fieldCount; int acumulatedLengthOfPreviousSentences = 0; float accelX, accelY, accelZ; char accelXArr[BYTELENGTH_OF_A_FLOAT], accelYArr[BYTELENGTH_OF_A_FLOAT], accelZArr[BYTELENGTH_OF_A_FLOAT]; float rotX, rotY, rotZ; char rotXArr[BYTELENGTH_OF_A_FLOAT], rotYArr[BYTELENGTH_OF_A_FLOAT], rotZArr[BYTELENGTH_OF_A_FLOAT]; float magX, magY, magZ; char magXArr[BYTELENGTH_OF_A_FLOAT], magYArr[BYTELENGTH_OF_A_FLOAT], magZArr[BYTELENGTH_OF_A_FLOAT]; float lat, lon, spd; char gpsLatArr[BYTELENGTH_OF_A_FLOAT], gpsLonArr[BYTELENGTH_OF_A_FLOAT], gpsSpdArr[BYTELENGTH_OF_A_FLOAT]; float temperature, humidity, voltage, current, pirAlarm; char temperatureArr[BYTELENGTH_OF_A_FLOAT], humidityArr[BYTELENGTH_OF_A_FLOAT], voltageArr[BYTELENGTH_OF_A_FLOAT], currentArr[BYTELENGTH_OF_A_FLOAT], pirAlarmArr[BYTELENGTH_OF_A_FLOAT]; float position, speed, time; char positionArr[BYTELENGTH_OF_A_FLOAT], speedArr[BYTELENGTH_OF_A_FLOAT], timeArr[BYTELENGTH_OF_A_FLOAT]; float demand, distance; char demandArr[BYTELENGTH_OF_A_FLOAT], distanceArr[BYTELENGTH_OF_A_FLOAT]; float channel; unsigned int termBigEnd; memset(PCTxData, 0, PC_TX_BUFFER_SIZE); //reset tx buffer for (i=0; i<numberOfSensors; i++) { switch (SIDs[i]) { case SID_9DOF_ACC: // 9DOF acc fieldCount = 3; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ); if (DUMMY) { if(accelX == 255.0f) { accelX = 0.0f; accelY = 0.0f; accelZ = 0.0f; } else { accelX++; accelY++; accelZ++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ); } memcpy(accelXArr, &accelX, BYTELENGTH_OF_A_FLOAT); memcpy(accelYArr, &accelY, BYTELENGTH_OF_A_FLOAT); memcpy(accelZArr, &accelZ, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF accel ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = accelXArr[j]; sendSentence[PC_FLOAT1_LOCATION+j] = accelYArr[j]; sendSentence[PC_FLOAT2_LOCATION+j] = accelZArr[j]; } lengthOfCurrentSentence = PC_9DOFACC_RESPONSE_LENGTH; break; case SID_9DOF_GYRO: // 9DOF gyro fieldCount = 3; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ); if (DUMMY) { if(rotX == 255.0f) { rotX = 0.0f; rotY = 0.0f; rotZ = 0.0f; } else { rotX++; rotY++; rotZ++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ); } memcpy(rotXArr, &rotX, BYTELENGTH_OF_A_FLOAT); memcpy(rotYArr, &rotY, BYTELENGTH_OF_A_FLOAT); memcpy(rotZArr, &rotZ, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF gyro ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = rotXArr[j]; sendSentence[PC_FLOAT1_LOCATION+j] = rotYArr[j]; sendSentence[PC_FLOAT2_LOCATION+j] = rotZArr[j]; } lengthOfCurrentSentence = PC_9DOFGYR_RESPONSE_LENGTH; break; case SID_9DOF_MAG: // 9DOF mag fieldCount = 3; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ); if (DUMMY) { if(magX == 255.0f) { magX = 0.0f; magY = 0.0f; magZ = 0.0f; } else { magX++; magY++; magZ++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ); } memcpy(magXArr, &magX, BYTELENGTH_OF_A_FLOAT); memcpy(magYArr, &magY, BYTELENGTH_OF_A_FLOAT); memcpy(magZArr, &magZ, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF mag ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = magXArr[j]; sendSentence[PC_FLOAT1_LOCATION+j] = magYArr[j]; sendSentence[PC_FLOAT2_LOCATION+j] = magZArr[j]; } lengthOfCurrentSentence = PC_9DOFMAG_RESPONSE_LENGTH; break; case SID_GPS: // GPS fieldCount = 3; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd); if (DUMMY) { if(lat == 255.0f) { lat = 0.0f; lon = 0.0f; spd = 0.0f; } else { lat++; lon++; spd++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd); } memcpy(gpsLatArr, &lat, BYTELENGTH_OF_A_FLOAT); memcpy(gpsLonArr, &lon, BYTELENGTH_OF_A_FLOAT); memcpy(gpsSpdArr, &spd, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //GPS ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = gpsLatArr[j]; sendSentence[PC_FLOAT1_LOCATION+j] = gpsLonArr[j]; sendSentence[PC_FLOAT2_LOCATION+j] = gpsSpdArr[j]; } lengthOfCurrentSentence = PC_GPS_RESPONSE_LENGTH; break; case SID_TEMP: // Temperature fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature); if (DUMMY) { if (temperature == 255.0f) { temperature = 0.0f; } else { temperature++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature); } memcpy(temperatureArr, &temperature, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Temperature ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = temperatureArr[j]; } lengthOfCurrentSentence = PC_TEMPERATURE_RESPONSE_LENGTH; break; case SID_HUM: // Humidity fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity); if (DUMMY) { if (humidity == 255.0f) { humidity = 0.0f; } else { humidity++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity); } memcpy(humidityArr, &humidity, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Humidity ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = humidityArr[j]; } lengthOfCurrentSentence = PC_HUMIDITY_RESPONSE_LENGTH; break; case SID_BATT_VOL: // Battery Voltage fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage); if (DUMMY) { if (voltage == 255.0f) { voltage = 0.0f; } else { voltage++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage); } memcpy(voltageArr, &voltage, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Voltage sensor ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = voltageArr[j]; } lengthOfCurrentSentence = PC_BATTVOLTAGE_RESPONSE_LENGTH; break; case SID_BATT_CUR: // Battery Current fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, ¤t); if (DUMMY) { if (current == 255.0f) { current = 0.0f; } else { current++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, ¤t); } memcpy(currentArr, ¤t, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Current sensor ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = currentArr[j]; } lengthOfCurrentSentence = PC_BATTCURRENT_RESPONSE_LENGTH; break; case SID_PIR: // PIR fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm); if (DUMMY) { if (pirAlarm == 255.0f) { pirAlarm = 0.0f; } else { pirAlarm++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm); } memcpy(pirAlarmArr, &pirAlarm, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //PIR ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = pirAlarmArr[j]; } lengthOfCurrentSentence = PC_PIR_RESPONSE_LENGTH; break; default: break; } if ((SIDs[i] >= SID_FIRSTSERVO_LOCATION)&& (SIDs[i] <= SID_LASTSERVO_LOCATION)) { //Servos fieldCount = 3; channel = 0.0f; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed); botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time); if (DUMMY) { if(position == 255.0f) { position = 0.0f; speed = 0.0f; time = 0.0f; } else { time++; time++; time++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed); botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time); } //SSC32Serial.printf("QP #%d \r", (char)channel); //while(servoPositionReturn == 0) {;} //position = servoPositionReturn; //servoPositionReturn = 0; memcpy(positionArr, &position, BYTELENGTH_OF_A_FLOAT); memcpy(speedArr, &speed, BYTELENGTH_OF_A_FLOAT); memcpy(timeArr, &time, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //servo ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = positionArr[j]; sendSentence[PC_FLOAT1_LOCATION+j] = speedArr[j]; sendSentence[PC_FLOAT2_LOCATION+j] = timeArr[j]; } lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH; } if ((SIDs[i] >= SID_FIRSTMOTOR_LOCATION)&& (SIDs[i] <= SID_LASTMOTOR_LOCATION)) { //Motors fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand); if (DUMMY) { if (demand == 255.0f) { demand = 0.0f; } else { demand++; } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand); } memcpy(demandArr, &demand, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Motor ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = demandArr[j]; } lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH; } if ((SIDs[i] >= SID_FIRSTUSOUND_LOCATION)&& (SIDs[i] <= SID_LASTUSOUND_LOCATION)) { //Ultrasounds fieldCount = 1; botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance); if (DUMMY) { /*if (distance == 255.0f) { distance = 0.0f; } else { distance++; }*/ // distance = (float)usound.distance(1); distance = -1.0f; if (DEBUG) { GPSSerial.printf("USOUND Distance (cm): %f\r\n", distance); } botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance); } memcpy(distanceArr, &distance, BYTELENGTH_OF_A_FLOAT); sendSentence[PC_CID_LOCATION] = (char)SID_MBED; sendSentence[PC_SID_LOCATION] = SIDs[i]; //Ultrasound ID sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount; for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) { sendSentence[PC_FLOAT0_LOCATION+j] = distanceArr[j]; } lengthOfCurrentSentence = PC_USOUND_RESPONSE_LENGTH; } memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], sendSentence, lengthOfCurrentSentence); acumulatedLengthOfPreviousSentences = acumulatedLengthOfPreviousSentences + lengthOfCurrentSentence; } // Convert from little endian to big endian: termBigEnd = PC_TERMINATION_PHRASE; termBigEnd = (termBigEnd>>BITLENGTH_OF_A_BYTE*3) | // Send byte 4 to byte 0 ((termBigEnd<<BITLENGTH_OF_A_BYTE) & 0x00ff0000) | // Send byte 2 to byte 3 ((termBigEnd>>BITLENGTH_OF_A_BYTE) & 0x0000ff00) | // Send byte 3 to btye 2 (termBigEnd<<BITLENGTH_OF_A_BYTE*3); // Send byte 0 to byte 4 memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], &termBigEnd, BYTELENGTH_OF_AN_INT); flags.txNewData = 1; } void parseRequestData(void) { char i,j; int bufferOffset = 0; // Tacking how far into the rx data we are int lengthOfCurrentSentence = 0; char CID; char SID; char fieldCount; char requestedSensorIDs[PC_REQUESTEDSID_MAXCOUNT] = {0}; // Servo vars: float channel; char posArr[BYTELENGTH_OF_A_FLOAT]; char spdArr[BYTELENGTH_OF_A_FLOAT]; char timArr[BYTELENGTH_OF_A_FLOAT]; float position; float speed; float time; // Motor vars: unsigned int pwm; float demand; char demArr[BYTELENGTH_OF_A_FLOAT]; //bool terminated; /* if(1) { terminated = 0; //GPSSerial.printf("Echo back..."); for (i=0; i<PC_RX_BUFFER_SIZE; i++) { if ( (PCRxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) + (PCRxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) + (PCRxData[i-2]<<BITLENGTH_OF_A_BYTE) + PCRxData[i-1] == PC_TERMINATION_PHRASE ) { terminated = 1; } if (!terminated) { GPSSerial.putc(PCRxData[i]); } } //GPSSerial.printf("...Echo end\r\n"); } */ for (j=0; j<((char)PC_SENTENCE_COUNT); j++) { // Read CID: CID = PCRxData[bufferOffset+PC_CID_LOCATION]; // Read SID: SID = PCRxData[bufferOffset+PC_SID_LOCATION]; // Read Field Count: fieldCount = PCRxData[bufferOffset+PC_FIELDCOUNT_LOCATION]; if(DEBUG) { GPSSerial.printf("Parsing data for SID: %d\r\n", SID); GPSSerial.printf("Field count: %d\r\n", fieldCount); } if (CID != SID_PC) break; // Stop as I'm only listening to the PC if (SID == SID_PC) break; // Stop as that message is for the PC if (SID > SID_LASTMOTOR_LOCATION) { flags.PCTimeout = 1; led3 = 1; break; // Stop as you cannot write to a sensor } if (SID == SID_MBED) { //Get ready to pump out sensor data: if(DEBUG) GPSSerial.printf("Sensor info requested\r\n"); for (i=0; i<fieldCount; i++) { // fieldCount = the number of requested sensors // Retrieve the SID of 'ith' requested sensor: requestedSensorIDs[i] = PCRxData[bufferOffset+PC_REQUESTEDSID_BYTE0_LOCATION+i]; if(DEBUG) GPSSerial.printf("Sensor requested: %d\r\n",requestedSensorIDs[i]); } // Send the SIDs to construct a response: constructTxData(requestedSensorIDs, fieldCount); // Length of this sentence is: lengthOfCurrentSentence = fieldCount + PC_CID_LENGTH + PC_SID_LENGTH + PC_FIELDCOUNT_LENGTH; } if ((SID >= SID_FIRSTSERVO_LOCATION)&& (SID <= SID_LASTSERVO_LOCATION)) { // We're writing to a servo if(DEBUG) GPSSerial.printf("Servo request\r\n"); // Retrieve byte values representing the position, speed and time floats: for (i=0; i<BYTELENGTH_OF_A_FLOAT; i++) { posArr[i] = PCRxData[(bufferOffset)+PC_FLOAT0_LOCATION+i]; // Float Byte 3, 4, 5 & 6 spdArr[i] = PCRxData[(bufferOffset)+PC_FLOAT1_LOCATION+i]; // Float Byte 7, 8, 9 & 10 timArr[i] = PCRxData[(bufferOffset)+PC_FLOAT2_LOCATION+i]; // Float Byte 11, 12, 13 & 14 } // Cast byte arrays into float values: position = *(float*)(posArr); speed = *(float*)(spdArr); time = *(float*)(timArr); if (DEBUG) { GPSSerial.printf("Position: %f\r\n", position); GPSSerial.printf("Speed: %f\r\n", speed); GPSSerial.printf("Time: %f\r\n", time); } // Find the channel associated with the desired servo: botData.getVal(SID, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Validate the position request (ensure it is within safe limits): if ((position >= (float)SERVO_POSITION_MIN)&& (position <= (float)SERVO_POSITION_MAX)) { // If no speed or time was specified: if ((speed == 0.0f)&&(time == 0.0f)) { SSC32Serial.printf("#%d P%d\r", (char)channel, (int)position); if(DEBUG) { GPSSerial.printf("writing to servo... "); GPSSerial.printf("#%d P%d\r\n", (char)channel, (int)position); } } // If a speed or time were specified: else { if (speed > 0.0f) { // Speed was specified: SSC32Serial.printf("#%d P%d S%d\r", (char)channel, (int)position, (int)speed); if(DEBUG) { GPSSerial.printf("writing to servo... "); GPSSerial.printf("#%d P%d S%d\r\n", (char)channel, (int)position, (int)speed); } } if (time > 0.0f) { // Time was specified: SSC32Serial.printf("#%d P%d T%d\r", (char)channel, (int)position, (int)time); if(DEBUG) { GPSSerial.printf("writing to servo... "); GPSSerial.printf("#%d P%d T%d\r\n", (char)channel, (int)position, (int)time); } } } // Update botData with new values: botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_POSITION, &position); botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed); botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_TIME, &time); } else { //led4 = 1; // Indicate an invalid data warning (position outside limits) } lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH; } if ((SID >= SID_FIRSTMOTOR_LOCATION)&& (SID <= SID_LASTMOTOR_LOCATION)) { // We're writing to a drive motor ESC //led4 = 0; // Retrieve byte values representing the demand float: demArr[0] = PCRxData[bufferOffset+PC_FLOAT0_LOCATION]; // Float Byte 0 demArr[1] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+1)]; // Float Byte 1 demArr[2] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+2)]; // Float Byte 2 demArr[3] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+3)]; // Float Byte 3 demand = *(float*)(demArr); // Cast byte array into a float value if(DEBUG) { GPSSerial.printf("Motor request...\r\n"); GPSSerial.printf("Demand: %f\r\n",demand); } if ((demand >= -1.0f)&& (demand <= 1.0f)) { // Ensure demand value is valid pwm = translateMotorDemandToPWM(demand); // Translate demand into a pwm value if(DEBUG) GPSSerial.printf("Pulsewidth (us): %d\r\n",pwm); // If request is for the nearside drive motor ESC: if (SID == (char)SID_DRIVE_NS) { driveNS.pulsewidth_us(pwm); // Write to nearside drive motor ESC // Update bot's DB with new demand value: botData.setVal( SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand); if(DEBUG) { GPSSerial.printf("Writing to NS motor (pwm value): "); GPSSerial.printf("%d \r\n", pwm); } } // If request is for the offside drive motor ESC: if (SID == (char)SID_DRIVE_OS) { driveOS.pulsewidth_us(pwm); // Wite to offside drive motor ESC // Update bot's DB with new demand value: botData.setVal(SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand); if(DEBUG) { GPSSerial.printf("Writing to OS motor (pwm value): "); GPSSerial.printf("%d \r\n", pwm); } } } else { //led4 = 1; // Indicate an invalid data warning (demand outside limits) } lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH; } // Update how far into the rx data we are using the current sentence length: bufferOffset = bufferOffset + lengthOfCurrentSentence; } } unsigned int translateMotorDemandToPWM(float demand) { /* Valid demand values: -1.0 upto 1.0 From protocol definition Desired pwm values: 0 upto 1.0 From ESC operation requirements The values resolve as follow: Demand: -1.0 | 0 | 1.0 M/S Ratio: 0 | 1 | 2.0 PWM: 700 | 1300 | 2000 Meaning: MaxReverse | Neutral | MaxForward */ float pwm = 0; pwm = demand+1; pwm = (625.0f*pwm)+700.0f; return (unsigned int)pwm; } void PCTimeout(void) { // Try telling the PC comms have been lost: //PCSerial.printf(PC_TIMEOUT_MESSAGE); if(DEBUG) { GPSSerial.printf(PC_TIMEOUT_MESSAGE); } //led1 = 1; // Indicate the MBED has timedout shutdown(); // Ensure the bot is in a safe shutdown mode/position // Wait for PC_TIMEOUT_MESSAGE to be sent before disabling the ISRs while(!SSC32Serial.txBufferEmpty()) {;} // Prevent any ISRs from firing: // (important this comes after the while else the timeout message won't send): __disable_irq(); // Should disable all ISRs NVIC_DisableIRQ(UART0_IRQn); // Directly turn off USB (PC)'s DMA ISRs while(1) {;} // Require a hard reset to resume } void shutdown(void) { float channel; // Temporary storage for locating servo channels // Set drive motor ESCs to neutral: driveNS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Nearside driveOS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Offside SSC32Serial.printf(SSC32_STOP); // Stop previous servo command // Find the channel for the arm's base servo: botData.getVal(SID_ARM_BASE, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the base in a 'safe' position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_ARM_BASE, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the arm's shoulder servo: botData.getVal(SID_ARM_SHOULDER, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the shoulder in a 'safe' position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_ARM_SHOULDER, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the arm's elbow servo: botData.getVal(SID_ARM_ELBOW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the elbow in a 'safe' position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_ARM_ELBOW, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the arm's wrist servo: botData.getVal(SID_ARM_WRIST, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the wrist in a 'safe' position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_ARM_WRIST, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the arm's manipulator servo: botData.getVal(SID_ARM_MANIPULATOR, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the manipulator in a 'safe' position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_ARM_MANIPULATOR, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the visions's pitch servo: botData.getVal(SID_VISION_PITCH, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the pitch in a neutral position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_VISION_PITCH, (int)SSC32_STOPPOSITION_TIME); // Find the channel for the visions's yaw servo: botData.getVal(SID_VISION_YAW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel); // Set the yaw in a neutral position: SSC32Serial.printf("#%d P%d T%d \r", (char)channel, (int)SSC32_STOPPOSITION_VISION_YAW, (int)SSC32_STOPPOSITION_TIME); } void dmaPCSerialRx(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; //Define local serial class if(flags.PCBegin) { if (flags.txSentData == 1) { PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); flags.txSentData = 0; } PCTimeoutTimer.start(); if (PCTimeoutTimer.read_ms() < PC_TIMEOUT_PERIOD) { incommingPCRxData[incommingPCRxDataCount] = serial->getc(); if(DEBUG) { GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]); } if (incommingPCRxDataCount > 2) { if ((incommingPCRxData[incommingPCRxDataCount-3]<<(BITLENGTH_OF_A_BYTE*3)) + (incommingPCRxData[incommingPCRxDataCount-2]<<(BITLENGTH_OF_A_BYTE*2)) + (incommingPCRxData[incommingPCRxDataCount-1]<<BITLENGTH_OF_A_BYTE) + (incommingPCRxData[incommingPCRxDataCount]) == PC_TERMINATION_PHRASE) { if (flags.rxNewData) { flags.PCTimeout = 1; led4 = 1; if(DEBUG) { GPSSerial.printf("Received new data too soon\r\n"); } PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); } else { memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer memcpy(PCRxData, incommingPCRxData, PC_RX_BUFFER_SIZE); //PCRxData = incommingPCRxData; memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer incommingPCRxDataCount = 0; flags.rxNewData = 1; PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); if(DEBUG) { GPSSerial.printf("\nReceived a termination\r\n"); } } } else { incommingPCRxDataCount++; } } else { incommingPCRxDataCount++; } } else { flags.PCTimeout = 1; led2 = 1; if(DEBUG) { GPSSerial.printf("Rx timeout\r\n"); } PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); } } else { incommingPCRxData[incommingPCRxDataCount] = serial->getc(); /* unsigned int lengthOfPhrase = 9;//sizeof(PC_BEGIN_PHRASE); if (incommingPCRxDataCount >= lengthOfPhrase) { if (string_compare(&PC_BEGIN_PHRASE, &(incommingPCRxData[incommingPCRxDataCount-(lengthOfPhrase-1)]), lengthOfPhrase)) { flags.PCBegin = 1; memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer incommingPCRxDataCount = 0; } else { incommingPCRxDataCount++; } } else { incommingPCRxDataCount++; } */ if (incommingPCRxDataCount > 7) { if ( (incommingPCRxData[incommingPCRxDataCount-8] == 'I') && (incommingPCRxData[incommingPCRxDataCount-7] == 'R') && (incommingPCRxData[incommingPCRxDataCount-6] == 'I') && (incommingPCRxData[incommingPCRxDataCount-5] == 'S') && (incommingPCRxData[incommingPCRxDataCount-4] == ':') && (incommingPCRxData[incommingPCRxDataCount-3] == 'I') && (incommingPCRxData[incommingPCRxDataCount-2] == 'N') && (incommingPCRxData[incommingPCRxDataCount-1] == 'I') && (incommingPCRxData[incommingPCRxDataCount] == 'T') ) { flags.PCBegin = 1; memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer incommingPCRxDataCount = 0; } else { incommingPCRxDataCount++; } } else { incommingPCRxDataCount++; } } } bool string_compare(char *a, char *b, unsigned int length) { for (unsigned int i=0; i<length; i++) { if (*a != *b) { return 0; } } return 1; } void sendPCSerialData(void) { int i; bool terminated = 0; for (i=0; i<PC_TX_BUFFER_SIZE; i++) { if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) + (PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) + (PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) + PCTxData[i-1] == PC_TERMINATION_PHRASE ) { terminated = 1; } if (!terminated) { PCSerial.putc(PCTxData[i]); } } if(DEBUG) { terminated = 0; GPSSerial.printf("Response back..."); for (i=0; i<PC_TX_BUFFER_SIZE; i++) { if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) + (PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) + (PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) + PCTxData[i-1] == PC_TERMINATION_PHRASE ) { terminated = 1; } if (!terminated) { GPSSerial.putc(PCTxData[i]); } } GPSSerial.printf("...Response end\r\n"); } PCTimeoutTimer.stop(); // precautionary - done after receiving PCTimeoutTimer.reset(); // precautionary - done after receiving flags.txSentData = 1; PCTimeoutTimer.start(); } void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; //Define local serial class servoPositionReturn = serial->getc(); } void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) { }