Terrabots / Mbed 2 deprecated DUMP_TRUCK_SPR2017

Main repository for dump truck API development in Spring 2017

Dependencies:   Tracker mbed MotorV2 SRF05 nRF24L01P

Fork of DUMP_TRUCK_TEST_V1 by Terrabots

Activities

This Week

If needed, please contact Milo Pan at mpan9@gatech.edu for rights and access to this repository.

DumpTruck.cpp@14:fd6090cddc2e, 2017-04-25 (annotated)

Committer:
simplyellow
Date:
Tue Apr 25 05:24:47 2017 +0000
Revision:
14:fd6090cddc2e
Parent:
13:112b6543909a
Child:
15:a4bbdde8ed69
communications final implementation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Josahty 0:6942f0e2198c 1 #include "DumpTruck.h"
simplyellow 14:fd6090cddc2e 2 #include <iostream>
simplyellow 14:fd6090cddc2e 3 #include <string>
simplyellow 13:112b6543909a 4
Josahty 0:6942f0e2198c 5 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
Josahty 0:6942f0e2198c 6 truckNumber = truckId;
simplyellow 14:fd6090cddc2e 7 //track = new Tracker(p29, p30, p15);
simplyellow 10:cf77da9be0b8 8 // PWM: 21, 22, 23
simplyellow 10:cf77da9be0b8 9 // DIR: 27, 28, 29
simplyellow 11:87f30625b213 10 frontMotor = new Motor(p23, p28);
simplyellow 13:112b6543909a 11 turnMotor = new Motor(p22, p29);
simplyellow 13:112b6543909a 12 bedMotor = new Motor(p23, p28);
simplyellow 10:cf77da9be0b8 13 //bed = new IMU(@@@@@@@);
simplyellow 11:87f30625b213 14 srf = new SRF05(p15, p16); //trigger, echo.
simplyellow 10:cf77da9be0b8 15 tooClose = false;
simplyellow 13:112b6543909a 16
simplyellow 12:502cf4fc6d98 17 nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
simplyellow 12:502cf4fc6d98 18 //transceiver set up, similar to one on base station code
simplyellow 12:502cf4fc6d98 19 txDataCnt = 0;
simplyellow 12:502cf4fc6d98 20 rxDataCnt = 0;
simplyellow 13:112b6543909a 21 received = false;
simplyellow 14:fd6090cddc2e 22 numCommands = sizeof(commands)/sizeof(*commands);
simplyellow 13:112b6543909a 23
simplyellow 14:fd6090cddc2e 24 commArr[0] = &DumpTruck::reportData;
simplyellow 14:fd6090cddc2e 25 commArr[1] = &DumpTruck::driveDistance;
simplyellow 14:fd6090cddc2e 26 commArr[2] = &DumpTruck::drive;
simplyellow 14:fd6090cddc2e 27 commArr[3] = &DumpTruck::turn;
simplyellow 14:fd6090cddc2e 28 commArr[4] = &DumpTruck::moveBed;
simplyellow 14:fd6090cddc2e 29 commArr[5] = &DumpTruck::stopBed;
simplyellow 14:fd6090cddc2e 30 commArr[6] = &DumpTruck::stopTurn;
simplyellow 14:fd6090cddc2e 31 commArr[7] = &DumpTruck::stopDrive;
simplyellow 12:502cf4fc6d98 32 }
simplyellow 12:502cf4fc6d98 33
simplyellow 12:502cf4fc6d98 34 void DumpTruck::startComms() {
simplyellow 14:fd6090cddc2e 35 //must call first in main.cpp
simplyellow 12:502cf4fc6d98 36 nrf->powerUp();
simplyellow 14:fd6090cddc2e 37 printf("\n\r--------\r\n");
simplyellow 14:fd6090cddc2e 38 printf("DUMP TRUCK\r\n");
simplyellow 14:fd6090cddc2e 39 printf("ID: %d \r\n", truckNumber);
simplyellow 14:fd6090cddc2e 40 printf("--------\r\n");
simplyellow 12:502cf4fc6d98 41 nrf->setTransferSize( TRANSFER_SIZE );
simplyellow 12:502cf4fc6d98 42 nrf->setReceiveMode();
simplyellow 12:502cf4fc6d98 43 nrf->enable();
simplyellow 13:112b6543909a 44
simplyellow 13:112b6543909a 45 // fill acked array
simplyellow 13:112b6543909a 46 for(int i = 0; i < TRANSFER_SIZE; i++) {
simplyellow 13:112b6543909a 47 acked[i] = '0';
simplyellow 13:112b6543909a 48 nacked[i] = '1';
simplyellow 13:112b6543909a 49 }
simplyellow 14:fd6090cddc2e 50
simplyellow 13:112b6543909a 51 }
simplyellow 13:112b6543909a 52
simplyellow 13:112b6543909a 53 void DumpTruck::sendAck() {
simplyellow 13:112b6543909a 54 wait(1);
simplyellow 13:112b6543909a 55 nrf->write(NRF24L01P_PIPE_P0, acked, TRANSFER_SIZE);
simplyellow 14:fd6090cddc2e 56 printf("ACK SENT\n\r");
simplyellow 13:112b6543909a 57 }
simplyellow 13:112b6543909a 58
simplyellow 13:112b6543909a 59 void DumpTruck::sendNack() {
simplyellow 13:112b6543909a 60 wait(1);
simplyellow 13:112b6543909a 61 nrf->write(NRF24L01P_PIPE_P0, nacked, TRANSFER_SIZE);
simplyellow 14:fd6090cddc2e 62 printf("NACK SENT\n\r");
simplyellow 12:502cf4fc6d98 63 }
simplyellow 12:502cf4fc6d98 64
simplyellow 12:502cf4fc6d98 65 void DumpTruck::getCommand() {
simplyellow 12:502cf4fc6d98 66 //called at the end of each executable command
simplyellow 12:502cf4fc6d98 67 //dump truck transceiver sends a code to base station, opening the channel
simplyellow 13:112b6543909a 68 //code is all 0's
simplyellow 13:112b6543909a 69 received = false;
simplyellow 13:112b6543909a 70 //acked to receive command
simplyellow 13:112b6543909a 71 sendAck();
simplyellow 13:112b6543909a 72 //possibly implement kill switch/stop current action
simplyellow 12:502cf4fc6d98 73 //code for receiving
simplyellow 12:502cf4fc6d98 74 while(!received) {
simplyellow 13:112b6543909a 75 if (nrf->readable()) {
simplyellow 13:112b6543909a 76 // ...read the data into the receive buffer
simplyellow 12:502cf4fc6d98 77 rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
simplyellow 13:112b6543909a 78 received = true; //after receiving command
simplyellow 13:112b6543909a 79 printf("received command\n\r");
simplyellow 13:112b6543909a 80 processCommand();
simplyellow 13:112b6543909a 81 }
simplyellow 13:112b6543909a 82 }
simplyellow 13:112b6543909a 83 }
simplyellow 12:502cf4fc6d98 84
simplyellow 13:112b6543909a 85 void DumpTruck::processCommand() {
simplyellow 14:fd6090cddc2e 86 printf("Processing command...\n\r");
simplyellow 14:fd6090cddc2e 87 int commandEnd = 99;
simplyellow 14:fd6090cddc2e 88 int valueEnd = 99;
simplyellow 14:fd6090cddc2e 89 for(int h = 0; h < TRANSFER_SIZE; h++) {
simplyellow 14:fd6090cddc2e 90 if(rxData[h] == ' ' || h == TRANSFER_SIZE - 1) {
simplyellow 14:fd6090cddc2e 91 if(commandEnd == 99) {
simplyellow 14:fd6090cddc2e 92 commandEnd = h + 1;
simplyellow 14:fd6090cddc2e 93 }
simplyellow 14:fd6090cddc2e 94 if((rxData[h+1] != ' ')&&(h < TRANSFER_SIZE-1)) {
simplyellow 14:fd6090cddc2e 95 valueEnd = h+1;
simplyellow 14:fd6090cddc2e 96 }
simplyellow 14:fd6090cddc2e 97 }
simplyellow 14:fd6090cddc2e 98 }
simplyellow 14:fd6090cddc2e 99 printf("%d, %d \r\n", commandEnd - 1, valueEnd);
simplyellow 14:fd6090cddc2e 100 char commandChar[commandEnd];
simplyellow 14:fd6090cddc2e 101 for(int g = 0; g < commandEnd; g++) {
simplyellow 14:fd6090cddc2e 102 commandChar[g] = rxData[g];
simplyellow 14:fd6090cddc2e 103 }
simplyellow 14:fd6090cddc2e 104
simplyellow 14:fd6090cddc2e 105 //limit user input value to 2 digits
simplyellow 14:fd6090cddc2e 106 // "turn 360"
simplyellow 14:fd6090cddc2e 107 //commandEnd - 1 = 4
simplyellow 14:fd6090cddc2e 108 //valueEnd = 5
simplyellow 14:fd6090cddc2e 109 // "drived 5"
simplyellow 14:fd6090cddc2e 110 //commandEnd = 6
simplyellow 14:fd6090cddc2e 111 //valueEnd = 7
simplyellow 14:fd6090cddc2e 112 int valSize = valueEnd - commandEnd + 1;
simplyellow 14:fd6090cddc2e 113 //printf("no cast %i, cast %i \r\n", rxData[valueEnd]-'0', (int)rxData[valueEnd]-'0');
simplyellow 14:fd6090cddc2e 114 if(valSize < 2) { //filters out commands with no # appended
simplyellow 14:fd6090cddc2e 115 if(valueEnd < 7) { //two digit
simplyellow 14:fd6090cddc2e 116 commandValue = 10*(int)((rxData[valueEnd])-'0') + (int)((rxData[valueEnd+1])-'0');
simplyellow 14:fd6090cddc2e 117 printf("2- command val is %i \r\n", commandValue);
simplyellow 14:fd6090cddc2e 118 } else { //one digit
simplyellow 14:fd6090cddc2e 119 commandValue = (int)(rxData[valueEnd]-'0');
simplyellow 14:fd6090cddc2e 120 printf("1- command val is %i \r\n", commandValue);
simplyellow 14:fd6090cddc2e 121 }
simplyellow 14:fd6090cddc2e 122 }
simplyellow 14:fd6090cddc2e 123
simplyellow 14:fd6090cddc2e 124 printf("There are %d commands available\n\r", sizeof(commands)/sizeof(*commands));
simplyellow 14:fd6090cddc2e 125 int f;
simplyellow 14:fd6090cddc2e 126 for(f = 0; f < numCommands; f++ ) {
simplyellow 14:fd6090cddc2e 127 if(strcmp(commandChar,commands[f])==32) {
simplyellow 14:fd6090cddc2e 128 printf("COMMAND #%i: ", f+1);
simplyellow 14:fd6090cddc2e 129 for(int a = 0; a < commandEnd; a++) {
simplyellow 14:fd6090cddc2e 130 printf("%c", commandChar[a]);
simplyellow 14:fd6090cddc2e 131 }
simplyellow 14:fd6090cddc2e 132 printf("\r\nORIGINAL: ");
simplyellow 14:fd6090cddc2e 133 for(int b = 0; b < TRANSFER_SIZE; b++) {
simplyellow 14:fd6090cddc2e 134 printf("%c", rxData[b]);
simplyellow 14:fd6090cddc2e 135 }
simplyellow 14:fd6090cddc2e 136 printf("\r\n");
simplyellow 14:fd6090cddc2e 137 // syntax: (this->*func)(foo, bar);
simplyellow 14:fd6090cddc2e 138 // commArr[0] = &DumpTruck::reportData;
simplyellow 14:fd6090cddc2e 139 (this->*commArr[f])();
simplyellow 13:112b6543909a 140 sendAck();
simplyellow 13:112b6543909a 141 break;
simplyellow 14:fd6090cddc2e 142 }
simplyellow 12:502cf4fc6d98 143 }
simplyellow 14:fd6090cddc2e 144 if(f == numCommands) {
simplyellow 14:fd6090cddc2e 145 //base station still interprets this as an ack because
simplyellow 14:fd6090cddc2e 146 //the dump truck tells it that it's ready to receive another command
simplyellow 14:fd6090cddc2e 147 sendNack();
simplyellow 14:fd6090cddc2e 148 printf("Command not recognized. \r\n");
simplyellow 14:fd6090cddc2e 149 }
simplyellow 14:fd6090cddc2e 150 printf("Waiting for next command\n\r");
simplyellow 13:112b6543909a 151 getCommand();
simplyellow 12:502cf4fc6d98 152 }
simplyellow 12:502cf4fc6d98 153
simplyellow 14:fd6090cddc2e 154 void DumpTruck::reportData(void) {
simplyellow 14:fd6090cddc2e 155 printf("Report Data \r\n");
simplyellow 12:502cf4fc6d98 156 //should run last in main thread
simplyellow 14:fd6090cddc2e 157 getCommand();
simplyellow 10:cf77da9be0b8 158 }
simplyellow 10:cf77da9be0b8 159
simplyellow 14:fd6090cddc2e 160 void DumpTruck::driveDistance() {
simplyellow 14:fd6090cddc2e 161 //float speed, float distance
simplyellow 14:fd6090cddc2e 162 printf("Drive distance: %d \r\n", commandValue);
simplyellow 10:cf77da9be0b8 163 frontMotor->write(speed);
simplyellow 13:112b6543909a 164 getCommand();
simplyellow 10:cf77da9be0b8 165 }
simplyellow 10:cf77da9be0b8 166
simplyellow 14:fd6090cddc2e 167 void DumpTruck::drive() {
simplyellow 14:fd6090cddc2e 168 //commandValue = speed etc
simplyellow 14:fd6090cddc2e 169 //float speed
simplyellow 14:fd6090cddc2e 170 printf("Drive speed: %d \r\n", commandValue);
simplyellow 10:cf77da9be0b8 171 frontMotor->write(speed);
simplyellow 13:112b6543909a 172 getCommand();
Josahty 0:6942f0e2198c 173 }
Josahty 0:6942f0e2198c 174
simplyellow 14:fd6090cddc2e 175 void DumpTruck::turn() {
simplyellow 14:fd6090cddc2e 176 //commandValue = angle etc
simplyellow 14:fd6090cddc2e 177 //float angle
simplyellow 14:fd6090cddc2e 178 printf("turn: %d degrees\r\n", commandValue);
simplyellow 13:112b6543909a 179 getCommand();
simplyellow 10:cf77da9be0b8 180 return;
simplyellow 10:cf77da9be0b8 181 }
simplyellow 10:cf77da9be0b8 182
simplyellow 14:fd6090cddc2e 183 void DumpTruck::moveBed() {
simplyellow 14:fd6090cddc2e 184 //bool upper, bool lower, float angle
simplyellow 14:fd6090cddc2e 185 printf("Move bed to angle: %d degrees \r\n", commandValue);
simplyellow 13:112b6543909a 186 getCommand();
simplyellow 10:cf77da9be0b8 187 return;
simplyellow 10:cf77da9be0b8 188 }
simplyellow 10:cf77da9be0b8 189
simplyellow 13:112b6543909a 190 void DumpTruck::stopDrive() {
simplyellow 14:fd6090cddc2e 191 printf("Stop drive \r\n");
simplyellow 11:87f30625b213 192 frontMotor->write(0);
simplyellow 13:112b6543909a 193 getCommand();
simplyellow 13:112b6543909a 194 }
simplyellow 13:112b6543909a 195
simplyellow 13:112b6543909a 196 void DumpTruck::stopTurn() {
simplyellow 14:fd6090cddc2e 197 printf("Stop turn \r\n");
simplyellow 13:112b6543909a 198 turnMotor->write(0);
simplyellow 13:112b6543909a 199 getCommand();
simplyellow 13:112b6543909a 200 }
simplyellow 13:112b6543909a 201
simplyellow 13:112b6543909a 202 void DumpTruck::stopBed() {
simplyellow 14:fd6090cddc2e 203 printf("Stop bed \r\n");
simplyellow 13:112b6543909a 204 bedMotor->write(0);
simplyellow 13:112b6543909a 205 getCommand();
simplyellow 10:cf77da9be0b8 206 }
simplyellow 10:cf77da9be0b8 207
simplyellow 10:cf77da9be0b8 208 bool DumpTruck::detect() {
simplyellow 10:cf77da9be0b8 209 proximity = srf->read();
simplyellow 11:87f30625b213 210 if(proximity < 20) { //cm
simplyellow 10:cf77da9be0b8 211 tooClose = true;
simplyellow 10:cf77da9be0b8 212 } else {
simplyellow 10:cf77da9be0b8 213 tooClose = false;
simplyellow 10:cf77da9be0b8 214 }
simplyellow 10:cf77da9be0b8 215 return tooClose;
simplyellow 14:fd6090cddc2e 216 }