diff --git a/Final Alex Firmware/Arduino/Alex_Arduino/Alex_Arduino.ino b/Final Alex Firmware/Arduino/Alex_Arduino/Alex_Arduino.ino index 012cf41..61c4c51 100644 --- a/Final Alex Firmware/Arduino/Alex_Arduino/Alex_Arduino.ino +++ b/Final Alex Firmware/Arduino/Alex_Arduino/Alex_Arduino.ino @@ -3,6 +3,7 @@ #include #include "packet.h" #include "constants.h" +#include "buffer.h" #include #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,11 +737,14 @@ 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(); } @@ -647,11 +752,11 @@ void right(float ang, float speed) 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();