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@16:6530f62dd28b, 2017-04-25 (annotated)

Committer:
simplyellow
Date:
Tue Apr 25 21:00:53 2017 +0000
Revision:
16:6530f62dd28b
Parent:
15:a4bbdde8ed69 
cleaned for doxygen

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 15:a4bbdde8ed69 10
simplyellow 15:a4bbdde8ed69 11 //pot pin 19 analogin
simplyellow 11:87f30625b213 12 frontMotor = new Motor(p23, p28);
simplyellow 13:112b6543909a 13 turnMotor = new Motor(p22, p29);
simplyellow 15:a4bbdde8ed69 14 bedMotor = new Motor(p24, p30);
simplyellow 15:a4bbdde8ed69 15 pot = new AnalogIn(p19);
simplyellow 10:cf77da9be0b8 16 //bed = new IMU(@@@@@@@);
simplyellow 15:a4bbdde8ed69 17 srf = new SRF05(p20, p21); //trigger, echo.
simplyellow 10:cf77da9be0b8 18 tooClose = false;
simplyellow 15:a4bbdde8ed69 19 interrupted = false;
simplyellow 13:112b6543909a 20
simplyellow 12:502cf4fc6d98 21 nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
simplyellow 12:502cf4fc6d98 22 //transceiver set up, similar to one on base station code
simplyellow 12:502cf4fc6d98 23 txDataCnt = 0;
simplyellow 12:502cf4fc6d98 24 rxDataCnt = 0;
simplyellow 13:112b6543909a 25 received = false;
simplyellow 14:fd6090cddc2e 26 numCommands = sizeof(commands)/sizeof(*commands);
simplyellow 13:112b6543909a 27
simplyellow 14:fd6090cddc2e 28 commArr[0] = &DumpTruck::reportData;
simplyellow 14:fd6090cddc2e 29 commArr[1] = &DumpTruck::driveDistance;
simplyellow 14:fd6090cddc2e 30 commArr[2] = &DumpTruck::drive;
simplyellow 14:fd6090cddc2e 31 commArr[3] = &DumpTruck::turn;
simplyellow 14:fd6090cddc2e 32 commArr[4] = &DumpTruck::moveBed;
simplyellow 14:fd6090cddc2e 33 commArr[5] = &DumpTruck::stopBed;
simplyellow 14:fd6090cddc2e 34 commArr[6] = &DumpTruck::stopTurn;
simplyellow 14:fd6090cddc2e 35 commArr[7] = &DumpTruck::stopDrive;
simplyellow 12:502cf4fc6d98 36 }
simplyellow 12:502cf4fc6d98 37
simplyellow 12:502cf4fc6d98 38 void DumpTruck::startComms() {
simplyellow 14:fd6090cddc2e 39 //must call first in main.cpp
simplyellow 15:a4bbdde8ed69 40 //zeroAngle = (float) pot.read();
simplyellow 15:a4bbdde8ed69 41 frontMotor->write(0);
simplyellow 15:a4bbdde8ed69 42 turnMotor->write(0);
simplyellow 15:a4bbdde8ed69 43 bedMotor->write(0);
simplyellow 12:502cf4fc6d98 44 nrf->powerUp();
simplyellow 14:fd6090cddc2e 45 printf("\n\r--------\r\n");
simplyellow 14:fd6090cddc2e 46 printf("DUMP TRUCK\r\n");
simplyellow 14:fd6090cddc2e 47 printf("ID: %d \r\n", truckNumber);
simplyellow 14:fd6090cddc2e 48 printf("--------\r\n");
simplyellow 12:502cf4fc6d98 49 nrf->setTransferSize( TRANSFER_SIZE );
simplyellow 12:502cf4fc6d98 50 nrf->setReceiveMode();
simplyellow 12:502cf4fc6d98 51 nrf->enable();
simplyellow 13:112b6543909a 52
simplyellow 13:112b6543909a 53 // fill acked array
simplyellow 13:112b6543909a 54 for(int i = 0; i < TRANSFER_SIZE; i++) {
simplyellow 13:112b6543909a 55 acked[i] = '0';
simplyellow 13:112b6543909a 56 nacked[i] = '1';
simplyellow 13:112b6543909a 57 }
simplyellow 13:112b6543909a 58 }
simplyellow 13:112b6543909a 59
simplyellow 13:112b6543909a 60 void DumpTruck::sendAck() {
simplyellow 13:112b6543909a 61 wait(1);
simplyellow 13:112b6543909a 62 nrf->write(NRF24L01P_PIPE_P0, acked, TRANSFER_SIZE);
simplyellow 14:fd6090cddc2e 63 printf("ACK SENT\n\r");
simplyellow 13:112b6543909a 64 }
simplyellow 13:112b6543909a 65
simplyellow 13:112b6543909a 66 void DumpTruck::sendNack() {
simplyellow 13:112b6543909a 67 wait(1);
simplyellow 13:112b6543909a 68 nrf->write(NRF24L01P_PIPE_P0, nacked, TRANSFER_SIZE);
simplyellow 14:fd6090cddc2e 69 printf("NACK SENT\n\r");
simplyellow 12:502cf4fc6d98 70 }
simplyellow 12:502cf4fc6d98 71
simplyellow 12:502cf4fc6d98 72 void DumpTruck::getCommand() {
simplyellow 12:502cf4fc6d98 73 //called at the end of each executable command
simplyellow 12:502cf4fc6d98 74 //dump truck transceiver sends a code to base station, opening the channel
simplyellow 13:112b6543909a 75 //code is all 0's
simplyellow 13:112b6543909a 76 received = false;
simplyellow 13:112b6543909a 77 //acked to receive command
simplyellow 13:112b6543909a 78 sendAck();
simplyellow 12:502cf4fc6d98 79 //code for receiving
simplyellow 12:502cf4fc6d98 80 while(!received) {
simplyellow 13:112b6543909a 81 if (nrf->readable()) {
simplyellow 13:112b6543909a 82 // ...read the data into the receive buffer
simplyellow 12:502cf4fc6d98 83 rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
simplyellow 13:112b6543909a 84 received = true; //after receiving command
simplyellow 13:112b6543909a 85 printf("received command\n\r");
simplyellow 13:112b6543909a 86 processCommand();
simplyellow 13:112b6543909a 87 }
simplyellow 13:112b6543909a 88 }
simplyellow 13:112b6543909a 89 }
simplyellow 12:502cf4fc6d98 90
simplyellow 13:112b6543909a 91 void DumpTruck::processCommand() {
simplyellow 14:fd6090cddc2e 92 printf("Processing command...\n\r");
simplyellow 14:fd6090cddc2e 93 int commandEnd = 99;
simplyellow 14:fd6090cddc2e 94 int valueEnd = 99;
simplyellow 14:fd6090cddc2e 95 for(int h = 0; h < TRANSFER_SIZE; h++) {
simplyellow 14:fd6090cddc2e 96 if(rxData[h] == ' ' || h == TRANSFER_SIZE - 1) {
simplyellow 14:fd6090cddc2e 97 if(commandEnd == 99) {
simplyellow 14:fd6090cddc2e 98 commandEnd = h + 1;
simplyellow 14:fd6090cddc2e 99 }
simplyellow 14:fd6090cddc2e 100 if((rxData[h+1] != ' ')&&(h < TRANSFER_SIZE-1)) {
simplyellow 14:fd6090cddc2e 101 valueEnd = h+1;
simplyellow 14:fd6090cddc2e 102 }
simplyellow 14:fd6090cddc2e 103 }
simplyellow 14:fd6090cddc2e 104 }
simplyellow 15:a4bbdde8ed69 105 //printf("%d, %d \r\n", commandEnd - 1, valueEnd);
simplyellow 14:fd6090cddc2e 106 char commandChar[commandEnd];
simplyellow 14:fd6090cddc2e 107 for(int g = 0; g < commandEnd; g++) {
simplyellow 14:fd6090cddc2e 108 commandChar[g] = rxData[g];
simplyellow 14:fd6090cddc2e 109 }
simplyellow 14:fd6090cddc2e 110
simplyellow 14:fd6090cddc2e 111 //limit user input value to 2 digits
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 15:a4bbdde8ed69 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 15:a4bbdde8ed69 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 15:a4bbdde8ed69 141 sendAck();
simplyellow 13:112b6543909a 142 break;
simplyellow 14:fd6090cddc2e 143 }
simplyellow 12:502cf4fc6d98 144 }
simplyellow 14:fd6090cddc2e 145 if(f == numCommands) {
simplyellow 14:fd6090cddc2e 146 //base station still interprets this as an ack because
simplyellow 14:fd6090cddc2e 147 //the dump truck tells it that it's ready to receive another command
simplyellow 14:fd6090cddc2e 148 sendNack();
simplyellow 14:fd6090cddc2e 149 printf("Command not recognized. \r\n");
simplyellow 14:fd6090cddc2e 150 }
simplyellow 14:fd6090cddc2e 151 printf("Waiting for next command\n\r");
simplyellow 13:112b6543909a 152 getCommand();
simplyellow 12:502cf4fc6d98 153 }
simplyellow 12:502cf4fc6d98 154
simplyellow 14:fd6090cddc2e 155 void DumpTruck::reportData(void) {
simplyellow 14:fd6090cddc2e 156 printf("Report Data \r\n");
simplyellow 15:a4bbdde8ed69 157 printf("Proximity %.2f\n\r", proximity);
simplyellow 15:a4bbdde8ed69 158 //printf("Dump Truck is turned %0.2f degrees", ((float) pot - zeroAngle) * 300);
simplyellow 12:502cf4fc6d98 159 //should run last in main thread
simplyellow 14:fd6090cddc2e 160 getCommand();
simplyellow 10:cf77da9be0b8 161 }
simplyellow 10:cf77da9be0b8 162
simplyellow 14:fd6090cddc2e 163 void DumpTruck::driveDistance() {
simplyellow 14:fd6090cddc2e 164 //float speed, float distance
simplyellow 14:fd6090cddc2e 165 printf("Drive distance: %d \r\n", commandValue);
simplyellow 15:a4bbdde8ed69 166 speed = (float) commandValue/10;
simplyellow 10:cf77da9be0b8 167 frontMotor->write(speed);
simplyellow 13:112b6543909a 168 getCommand();
simplyellow 10:cf77da9be0b8 169 }
simplyellow 10:cf77da9be0b8 170
simplyellow 14:fd6090cddc2e 171 void DumpTruck::drive() {
simplyellow 14:fd6090cddc2e 172 //commandValue = speed etc
simplyellow 14:fd6090cddc2e 173 //float speed
simplyellow 15:a4bbdde8ed69 174 speed = (float) commandValue/10;
simplyellow 14:fd6090cddc2e 175 printf("Drive speed: %d \r\n", commandValue);
simplyellow 10:cf77da9be0b8 176 frontMotor->write(speed);
simplyellow 13:112b6543909a 177 getCommand();
Josahty 0:6942f0e2198c 178 }
Josahty 0:6942f0e2198c 179
simplyellow 14:fd6090cddc2e 180 void DumpTruck::turn() {
simplyellow 14:fd6090cddc2e 181 //commandValue = angle etc
simplyellow 14:fd6090cddc2e 182 //float angle
simplyellow 14:fd6090cddc2e 183 printf("turn: %d degrees\r\n", commandValue);
simplyellow 15:a4bbdde8ed69 184 turnMotor->write(0.6f);
simplyellow 15:a4bbdde8ed69 185 wait(1);
simplyellow 15:a4bbdde8ed69 186 turnMotor->write(0);
simplyellow 13:112b6543909a 187 getCommand();
simplyellow 10:cf77da9be0b8 188 return;
simplyellow 10:cf77da9be0b8 189 }
simplyellow 10:cf77da9be0b8 190
simplyellow 14:fd6090cddc2e 191 void DumpTruck::moveBed() {
simplyellow 14:fd6090cddc2e 192 //bool upper, bool lower, float angle
simplyellow 14:fd6090cddc2e 193 printf("Move bed to angle: %d degrees \r\n", commandValue);
simplyellow 15:a4bbdde8ed69 194 if(commandValue > 500) {
simplyellow 15:a4bbdde8ed69 195 bedMotor->write(0.9f);
simplyellow 15:a4bbdde8ed69 196 } else {
simplyellow 15:a4bbdde8ed69 197 bedMotor->write(-0.1f);
simplyellow 15:a4bbdde8ed69 198 }
simplyellow 15:a4bbdde8ed69 199 wait(1);
simplyellow 15:a4bbdde8ed69 200 bedMotor->write(0);
simplyellow 13:112b6543909a 201 getCommand();
simplyellow 10:cf77da9be0b8 202 return;
simplyellow 10:cf77da9be0b8 203 }
simplyellow 10:cf77da9be0b8 204
simplyellow 13:112b6543909a 205 void DumpTruck::stopDrive() {
simplyellow 14:fd6090cddc2e 206 printf("Stop drive \r\n");
simplyellow 11:87f30625b213 207 frontMotor->write(0);
simplyellow 13:112b6543909a 208 getCommand();
simplyellow 13:112b6543909a 209 }
simplyellow 13:112b6543909a 210
simplyellow 15:a4bbdde8ed69 211 void DumpTruck::detectStop() {
simplyellow 15:a4bbdde8ed69 212 printf("Stopped all systems \r\n");
simplyellow 15:a4bbdde8ed69 213 frontMotor->write(0);
simplyellow 15:a4bbdde8ed69 214 bedMotor->write(0);
simplyellow 15:a4bbdde8ed69 215 turnMotor->write(0);
simplyellow 15:a4bbdde8ed69 216 }
simplyellow 15:a4bbdde8ed69 217
simplyellow 13:112b6543909a 218 void DumpTruck::stopTurn() {
simplyellow 14:fd6090cddc2e 219 printf("Stop turn \r\n");
simplyellow 13:112b6543909a 220 turnMotor->write(0);
simplyellow 13:112b6543909a 221 getCommand();
simplyellow 13:112b6543909a 222 }
simplyellow 13:112b6543909a 223
simplyellow 13:112b6543909a 224 void DumpTruck::stopBed() {
simplyellow 14:fd6090cddc2e 225 printf("Stop bed \r\n");
simplyellow 13:112b6543909a 226 bedMotor->write(0);
simplyellow 13:112b6543909a 227 getCommand();
simplyellow 10:cf77da9be0b8 228 }
simplyellow 10:cf77da9be0b8 229
simplyellow 10:cf77da9be0b8 230 bool DumpTruck::detect() {
simplyellow 10:cf77da9be0b8 231 proximity = srf->read();
simplyellow 15:a4bbdde8ed69 232 if(proximity < 10) { //cm
simplyellow 10:cf77da9be0b8 233 tooClose = true;
simplyellow 15:a4bbdde8ed69 234 // detectStop();
simplyellow 10:cf77da9be0b8 235 } else {
simplyellow 10:cf77da9be0b8 236 tooClose = false;
simplyellow 10:cf77da9be0b8 237 }
simplyellow 10:cf77da9be0b8 238 return tooClose;
simplyellow 14:fd6090cddc2e 239 }