Add files via upload

Basically just moved code into new tabs so you can read it better

Team6_RBE2001_CSUpdates/BTComms.cpp

@@ -0,0 +1,135 @@
+#include "BTComms.h"
+
+#include "Arduino.h"
+
+/**
+ * Bluetooth communications constructor
+ */
+BTComms::BTComms() {
+	messageIndex = 0;
+	messageLength = 0;
+	BTstate = kLookingForStart;
+}
+
+/**
+ * Code that is called from the arduino setup() function
+ * This initializes things that cannot be set up from the constructor.
+ */
+void BTComms::setup() {
+  Serial3.begin(115200);
+}
+
+/**
+ * Send a message to the RCS that has 3 values (opcode, source, dest)
+ * This method sends messages via bluetooth to the field that have an opcode with
+ * a source and destination address. It is used for the heartbeat message that has
+ * no message data.
+ *
+ * You could add additional methods exactly like this one, that take a payload such
+ * as a status message. You can create a new method that is exactly the same as this
+ * one (also called writeMessage), but with an additional parameter that gets sent.
+ * With C++ you can have multiple methods with the same name that are different by
+ * the number of parameters they have. Be sure that the new function adjusts the length,
+ * and writes the extra byte to the bluetooth interface and includes it in the checksum
+ * calculation.
+ */void BTComms::writeMessage(unsigned char opcode, unsigned char source, unsigned char dest) {
+  Serial3.write(kMessageStart);
+  Serial3.write(5);
+  Serial3.write(opcode);
+  Serial3.write(source);
+  Serial3.write(dest);
+  Serial3.write(0xff - (opcode + source + dest + 5));
+}
+ /**
+  * Send a message to the RCS that has 4 values (opcode, source, dest, rod)
+  * This method sends messages via bluetooth to the field that have an opcode with
+  * a source, a destination address.. It is used for the radiataion alert that has
+  * rod data, determining if it has a spent or new rod.
+  */
+void BTComms::writeMessage(unsigned char opcode, unsigned char source, unsigned char dest, unsigned char rod) {
+  Serial3.write(kMessageStart);
+  Serial3.write(6);
+  Serial3.write(opcode);
+  Serial3.write(source);
+  Serial3.write(dest);
+  Serial3.write(rod);
+  Serial3.write(0xff - (opcode + source + dest + rod + 6));
+}
+/**
+ * Get the length of the currently received message
+ * @returns int The number of bytes in the received message
+ */
+int BTComms::getMessageLength() {
+  return messageLength;
+}
+
+/**
+ * Get a byte from the current message
+ * Retrieve a byte from the currently received message. Only a single message is
+ * remembered at any time, so you have to call read(), notice that there is a message,
+ * and then do something with the message bytes.
+ * @param index The offset (zero-based) to the byte to be returned
+ * @returns unsigned char The byte that is at the specified index
+ */
+unsigned char BTComms::getMessageByte(unsigned index) {
+  if (index >= messageLength) {
+    Serial.print("request for message byte beyond end of message ");
+    Serial.println(index);
+    return 0;
+  }
+  return message[index];  
+}
+
+/**
+ * Read a message from Bluetooth
+ * This method reads messages from Bluetooth by looking for the message start byte, then
+ * reading the message length, checksum and destination verification and data.
+ */
+bool BTComms::read() {
+  while (Serial3.available()) {
+    unsigned inByte = Serial3.read();
+    switch (BTstate) {
+      case kLookingForStart:
+        if (inByte != kMessageStart)
+        	break;
+        BTstate = kMessageCheck;
+        break;
+      case kMessageCheck:
+        messageLength = inByte - 1;
+        if (messageLength >= messageBufferLength) {
+          Serial.println("Received message length greater than buffer size");
+          BTstate = kLookingForStart;
+          break;
+        }
+        checkSum=(0xFF)-(message[(messageLength-1)])-(message[(messageLength-2)])-(message[(messageLength-3)])-(message[(messageLength-4)]);
+        destination=(message[(messageLength-1)]);
+        if(messageLength==6){
+          checkSum=checkSum-(message[(messageLength-5)]);
+          destination=(message[(messageLength-2)]);
+          }
+        if (((checkSum),HEX) != (message[messageLength],HEX)){
+          Serial.println("Invalid checksum");
+          BTstate = kLookingForStart;
+          break;
+          }
+        if(((destination,HEX) != (0,HEX)) && ((destination,HEX) != (6,HEX))){
+          Serial.println("invalid destination");
+          BTstate = kLookingForStart;
+          break;
+          }
+        messageIndex = 0;
+        BTstate = kReadMessage;
+        break; 
+      case kReadMessage:
+        message[messageIndex++] = inByte;
+        if (messageIndex >= messageLength) {
+          BTstate = kLookingForStart;
+          return true;
+        }
+        break;
+       default:
+        Serial.println("Invalid state");
+    }}
+  return false;
+}
+

Team6_RBE2001_CSUpdates/BTComms.h

@@ -0,0 +1,35 @@
+#ifndef _BTReader
+#define _BTReader
+
+/**
+ * Low level class to both receive and send message to the field.
+ * The writeMessage() method sends messages to the field and the other methods
+ * receive message from the field.
+ *
+ * This class is used by the higher level Messages class to separate the actual
+ * byte reading and dealing with checksums from the messages class to make it more
+ * understandable.
+ */
+class BTComms {
+  public:
+    BTComms();
+    void setup();
+    int getMessageLength();
+    unsigned char getMessageByte(unsigned index);
+    bool read();
+    void writeMessage(unsigned char b1, unsigned char b2, unsigned char b3);
+    void writeMessage(unsigned char b1, unsigned char b2, unsigned char b3, unsigned char b4);
+   private:
+    enum BTstate {kLookingForStart, kMessageCheck, kReadMessage} BTstate;
+    unsigned messageLength;
+    static const int messageBufferLength = 20; 
+    unsigned char message[messageBufferLength];
+    unsigned messageIndex;
+    int checkSum;
+    unsigned char kMessageStart = 0x5f;
+    int currentBit; 
+    int destination;
+
+};
+
+#endif

Team6_RBE2001_CSUpdates/BTFunctions.ino

@@ -0,0 +1,49 @@
+/**
+ * This "starter" program reads and debug-prints messages from the field. It also sends
+ * heartbeat messages every second letting the field know that the robot is still running.
+ *
+ * You can use this as the basis for your project, or copy the elements from here into your
+ * project. If you do that, be sure that you include the Messages and BTComms classes (.cpp
+ * and .h files) into your new project.
+ */
+//this function sets three timers
+void setTimers(){
+  stepTimer1=millis()+500;
+  stepTimer2=millis()+1000;
+  stepTimer3=millis()+1500;
+}
+//this function checks the deposit storage at the current line
+bool storage(int currentLine){
+  int bitCalc=msg.storageAvailability();
+  if(currentLine==4){
+    return (bitCalc<8);
+  }
+  else if(currentLine==3){
+    return ((bitCalc<3)||(12>bitCalc>7));
+  }
+  else if(currentLine==2){
+    return ((bitCalc<2)||(6>bitCalc>3)||(10>bitCalc>7)||(14>bitCalc>11));
+  }
+  else if(currentLine==1){
+    return ((bitCalc==0)||(bitCalc==2)||(bitCalc==4)||(bitCalc==6)||(bitCalc==8)||(bitCalc==10)||(bitCalc==12)||(bitCalc==14));
+  }
+}
+//this function checks the supply storage at the current line
+bool supply(int currentLine){
+  int bitCalc=msg.supplyAvailability();
+  if(currentLine==4){
+    return (bitCalc<8);
+  }
+  else if(currentLine==3){
+    return ((bitCalc<3)||(12>bitCalc>7));
+  }
+  else if(currentLine==2){
+    return ((bitCalc<2)||(6>bitCalc>3)||(10>bitCalc>7)||(14>bitCalc>11));
+  }
+  else if(currentLine==1){
+    return ((bitCalc==0)||(bitCalc==2)||(bitCalc==4)||(bitCalc==6)||(bitCalc==8)||(bitCalc==10)||(bitCalc==12)||(bitCalc==14));
+  }
+  else{
+    return false;
+  }
+}

Team6_RBE2001_CSUpdates/HelperFunctions.ino

@@ -0,0 +1,90 @@
+//this function should have robot follow a line
+void lineFollowing(){
+  qtra.read(sensorValues);
+  linePos=sensorValues[3]-(sensorValues[4]+320);
+  analogWrite(leftDriveBack,0);
+  analogWrite(leftDriveFor,150-(linePos*linekp));
+  analogWrite(rightDriveBack,0);
+  analogWrite(rightDriveFor,150+(linePos*linekp));
+}
+//this function should have robot turn left
+void turnLeft(){
+  qtra.read(sensorValues);
+  analogWrite(leftDriveBack,120);
+  analogWrite(leftDriveFor,0);
+  analogWrite(rightDriveBack,0);
+  analogWrite(rightDriveFor,120);
+}
+//this function should have robot turn right
+void turnRight(){
+  qtra.read(sensorValues);
+  analogWrite(leftDriveBack,0);
+  analogWrite(leftDriveFor,120);
+  analogWrite(rightDriveBack,120);
+  analogWrite(rightDriveFor,0);
+}
+//this function has the robot drive backwards
+void backwards(){
+  qtra.read(sensorValues);
+  linePos=sensorValues[3]-(sensorValues[4]+320);
+  analogWrite(leftDriveBack,150+(linePos*linekp));
+  analogWrite(leftDriveFor,0);
+  analogWrite(rightDriveBack,150-(linePos*linekp));
+  analogWrite(rightDriveFor,0);
+}
+//this function should stop the drive motors
+void driveStop(){
+  analogWrite(leftDriveBack,0);
+  analogWrite(leftDriveFor,0);
+  analogWrite(rightDriveBack,0);
+  analogWrite(rightDriveFor,0);
+}
+//raises arm to horizontal position using PID
+void armRaise(){
+  currentValue=analogRead(armPotPin);
+  error=armRaisedValue-currentValue;
+  if(error>0){
+  analogWrite(armMotorRaise, (error*kp));
+  analogWrite(armMotorLower, 0);
+  }
+  else if(error<0){
+  analogWrite(armMotorLower, (error*kp*-1));
+  analogWrite(armMotorRaise, 0);
+  }
+}
+//lowers arm to vertical position using PID
+void armLower(){
+  currentValue=analogRead(armPotPin);
+  error=armLoweredValue-currentValue;
+  if(error>0){
+  analogWrite(armMotorRaise, (error*kp));
+  analogWrite(armMotorLower, 0);
+  }
+  else if(error<0){
+  analogWrite(armMotorLower, (error*kp*-1));
+  analogWrite(armMotorRaise, 0);
+  }
+}
+void armStop(){
+  analogWrite(armMotorRaise, 0);
+  analogWrite(armMotorLower, 0);
+}
+//extend linear slide 
+void slideExtend(){
+  slideMotor.write(0);
+}
+//recline linear slide
+void slideRecline(){
+  slideMotor.write(0);
+}
+//close gripper to pick up rod
+void gripClose(){
+  gripMotor.write(131);
+  digitalWrite(RadLED, HIGH);
+  }
+//opens gripper to release rod
+void gripOpen(){
+  gripMotor.write(60);
+  digitalWrite(RadLED, LOW);
+  currentRod=0;
+  }