Alex_Arduino_Full_Bare_Metal.ino

Final Alex Firmware/Arduino/Alex_Arduino/Alex_Arduino.ino

@@ -3,6 +3,7 @@
 #include <avr/sleep.h> 
 #include "packet.h"
 #include "constants.h"
+#include "buffer.h"
 #include <math.h>
 
 #define _STOP 0
@@ -34,6 +35,10 @@
  //Pin 10: OC1B (PB2) BIN1 BOUT1 RED RIGHT MOTOR
  //Pin 11: OC2A (PB3) BIN2 BOUT2 BLACK RIGHT MOTOR
 
+#define RECV_SIZE      128
+
+static TBuffer _recvBuffer;
+
 
 typedef enum{
   STOP = 0,
@@ -338,9 +343,22 @@ ISR(INT1_vect){
 void setupSerial()
 {
   // To replace later with bare-metal.
-  Serial.begin(9600);
+  //Serial.begin(9600);
+  UBRR0L = 103;
+  UBRR0H = 0;
+
+  UCSR0C = 0b00000110; 
+  UCSR0A = 0;
 }
 
+void setupBuffers()
+{
+    // Initialize the receive and transmit buffers.
+    initBuffer(&_recvBuffer, RECV_SIZE);
+}
+
+
+
 // Start the serial connection. For now we are using
 // Arduino wiring and this function is empty. We will
 // replace this later with bare-metal code.
@@ -349,30 +367,62 @@ void startSerial()
 {
   // Empty for now. To be replaced with bare-metal code
   // later on.
-
+  UCSR0B = 0b10111000;
+}
+
+ISR(USART_RX_vect) {
+    // Write received data
+    unsigned char data = UDR0;
+    writeBuffer(&_recvBuffer, data);
 }
 
 // Read the serial port. Returns the read character in
 // ch if available. Also returns TRUE if ch is valid. 
 // This will be replaced later with bare-metal code.
 
-int readSerial(char *buffer)
+int readSerial(unsigned char* line)
 {
 
-  int count=0;
+    /*
+    int count=0;
+    while(Serial.available())
+      buffer[count++] = Serial.read();
+    return count;
+
+    */
+    int count = 0;
+
+    TBufferResult result;
 
-  while(Serial.available())
-    buffer[count++] = Serial.read();
+    do {
+        result = readBuffer(&_recvBuffer, &line[count]);
+        if (result == BUFFER_OK)
+            count++;
+    } while (result == BUFFER_OK);
 
-  return count;
+    return count;
 }
 
 // Write to the serial port. Replaced later with
 // bare-metal code
 
-void writeSerial(const char *buffer, int len)
-{
-  Serial.write(buffer, len);
+void writeSerial(const unsigned char* line, int len) {
+    //Serial.write(buffer, len);
+
+    /*
+    while (len--) {
+        while ((UCSR0A & 0b00100000) == 0);
+        UDR0 = *line;
+        line++;
+    }
+    */
+   TResult result = BUFFER_OK;
+   for(int i = 0; i < len && result == BUFFER_OK; i++)
+   {
+    result = writeBuffer(&_xmitBuffer, line[i]);
+   }
+ UDR0 = line[0];
+ UCSR0B |= 0b00100000;
 }
 
 /*
@@ -391,16 +441,46 @@ void setupMotors()
    *    B1IN - Pin 10, PB2, OC1B
    *    B2In - pIN 11, PB3, OC2A
    */
+
+ //Bare-metal
+ //Set Pin5 and Pin6 as output
+ DDRD |= ((1 << PIN6) | (1 << PIN5));
+
+ //Setup PWM
+ TCNT0 = 0;
+ TIMSK0 |= 0b110; //OCIEA = 1, OCIEB = 1
+ OCR0A = 128;
+ OCR0B = 128;
+ TCCR0B = 0b00000011; //clk64
 }
 
 // Start the PWM for Alex's motors.
 // We will implement this later. For now it is
 // blank.
-void startMotors()
+//void startMotors()
+void right_motor_forward()
 {
-
+ TCCR0A = 0b10000001;
+}
+
+void right_motor_reverse()
+{
+ TCCR0A = 0b00100001;
+}
+
+void left_motor_forward()
+{
+ TCCR0A = 0b01000001;
 }
 
+void left_motor_reverse()
+{
+ TCCR0A = 0b00010001;
+}
+
+ISR(TIMER0_COMPA_vect){}
+ISR(TIMER0_COMPB_vect){}
+
 // Convert percentages to PWM values
 int pwmVal(float speed)
 {
@@ -460,7 +540,10 @@ void calibrateMotors(){
         curr_pwm = (val>255)?255:
                    (val < 0)?0:
                    val;
-        analogWrite(RF, curr_pwm);
+        //analogWrite(RF, curr_pwm);
+       //Bare metal
+       OCR0B = curr_pwm;
+       right_motor_forward();
       }
     break;
 
@@ -480,7 +563,10 @@ void calibrateMotors(){
         curr_pwm = (val>255)?255:
                    (val < 0)?0:
                    val;
-        analogWrite(RR, curr_pwm);
+        //analogWrite(RR, curr_pwm);
+       //Bare metal
+       OCR0B = curr_pwm;
+       right_motor_reverse();
       }
     break;
 
@@ -499,7 +585,10 @@ void calibrateMotors(){
         curr_pwm = (val>255)?255:
                    (val < 0)?0:
                    val;
-        analogWrite(RR, curr_pwm);
+        //analogWrite(RR, curr_pwm);
+       //Bare metal
+       OCR0B = curr_pwm;
+       right_motor_reverse();
       }
     break;
 
@@ -518,7 +607,10 @@ void calibrateMotors(){
         curr_pwm = (val>255)?255:
                    (val < 0)?0:
                    val;
-        analogWrite(RF, curr_pwm);
+        //analogWrite(RF, curr_pwm);
+       //Bare metal
+       OCR0B = curr_pwm;
+       right_motor_forward();
       }
     break;
 
@@ -549,10 +641,14 @@ void forward(float dist, float speed)
   if(dist > 0) deltaDist = dist;
   else deltaDist = 9999999;
   newDist = forwardDist + deltaDist;
-  analogWrite(LF, val);
-  analogWrite(RF, curr_pwm);
-  analogWrite(LR, 0);
-  analogWrite(RR, 0);
+  //analogWrite(LF, val);
+  //analogWrite(RF, curr_pwm);
+  //analogWrite(LR, 0);
+  //analogWrite(RR, 0);
+ OCR0A = val;
+ OCR0B = curr_pwm;
+ right_motor_forward();
+ left_motor_forward();
 }
 
 // Reverse Alex "dist" cm at speed "speed".
@@ -576,11 +672,14 @@ void reverse(float dist, float speed)
   // LF = Left forward pin, LR = Left reverse pin
   // RF = Right forward pin, RR = Right reverse pin
   // This will be replaced later with bare-metal code.
-  analogWrite(LR, val);
-  analogWrite(RR, curr_pwm);
-  analogWrite(LF, 0);
-  analogWrite(RF, 0);
-
+  //analogWrite(LR, val);
+  //analogWrite(RR, curr_pwm);
+  //analogWrite(LF, 0);
+  //analogWrite(RF, 0);
+ OCR0A = val;
+ OCR0B = curr_pwm;
+ right_motor_reverse();
+ left_motor_reverse();
 
 }
 
@@ -609,11 +708,14 @@ void left(float ang, float speed)
   // We will also replace this code with bare-metal later.
   // To turn left we reverse the left wheel and move
   // the right wheel forward.
-  analogWrite(RF, curr_pwm);
-  analogWrite(LR, val);
-  analogWrite(RR, 0);
-  analogWrite(LF, 0);
-
+  //analogWrite(RF, curr_pwm);
+  //analogWrite(LR, val);
+  //analogWrite(RR, 0);
+  //analogWrite(LF, 0);
+ OCR0A = val;
+ OCR0B = curr_pwm;
+ right_motor_forward();
+ left_motor_reverse();
 }
 
 // Turn Alex right "ang" degrees at speed "speed".
@@ -635,23 +737,26 @@ void right(float ang, float speed)
   // We will also replace this code with bare-metal later.
   // To turn right we reverse the right wheel and move
   // the left wheel forward.
-  analogWrite(RR, curr_pwm);
-  analogWrite(LF, val);
-  analogWrite(RF, 0);
-  analogWrite(LR, 0);
-
+  //analogWrite(RR, curr_pwm);
+  //analogWrite(LF, val);
+  //analogWrite(RF, 0);
+  //analogWrite(LR, 0);
+ OCR0A = val;
+ OCR0B = curr_pwm;
+ right_motor_reverse();
+ left_motor_forward();
 }
 
 
 // Stop Alex. To replace with bare-metal code later.
 void stop()
 {
   dir = STOP;
-  analogWrite(LF, 0);
-  analogWrite(LR, 0);
-  analogWrite(RF, 0);
-  analogWrite(RR, 0);
-
+  //analogWrite(LF, 0);
+  //analogWrite(LR, 0);
+  //analogWrite(RF, 0);
+  //analogWrite(RR, 0);
+ TCCR0A = 0b00000001;
 }
 
 /*
@@ -818,7 +923,7 @@ void setup() {
   setupSerial();
   startSerial();
   setupMotors();
-  startMotors();
+  //startMotors();
   enablePullups();
   initializeState();
   setupPowerSaving();