Mercurial > ukubik
changeset 77:c104b318ec34
Renamed servo.ino to ukubik.ino
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Mon, 15 Sep 2014 20:05:00 +0200 |
| parents | a16884812b83 |
| children | 819e32b9495c |
| files | fold.py servo/accel.ino servo/battery.ino servo/beep.ino servo/fastFunctions.ino servo/legs.ino servo/servo.ino servo/servos.ino ukubik/accel.ino ukubik/battery.ino ukubik/beep.ino ukubik/legs.ino ukubik/servos.ino ukubik/ukubik.ino |
| diffstat | 14 files changed, 303 insertions(+), 432 deletions(-) [+] |
line wrap: on
line diff
--- a/fold.py Sun Sep 14 13:12:28 2014 +0200 +++ b/fold.py Mon Sep 15 20:05:00 2014 +0200 @@ -2,13 +2,5 @@ # -*- coding: utf-8 -*- import serial -import kubik - -robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 115200)) -for leg in robot.legs: - leg.hip.move_degrees(45 * leg.y_dir) - leg.knee.move_degrees(-90) - leg.ankle.move_degrees(90) - for joint in leg: - joint.update() +serial.Serial('/dev/rfcomm0', 115200).write('\xff\xf3')
--- a/servo/accel.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,53 +0,0 @@ -// 3.3V maximum for the acceleration sensor. -#define ACCEL_MAX 676 - -// How many acceleration measurements to cache. -#define MAX_ACCEL_BUFFER 16 - - -int accel_buffer[MAX_ACCEL_BUFFER][3]; - - -void read_accel() { - static unsigned char accel_buffer_index = 0; - accel_buffer[accel_buffer_index][0] = analogRead(A2); - accel_buffer[accel_buffer_index][1] = analogRead(A1); - accel_buffer[accel_buffer_index][2] = analogRead(A0); - accel_buffer_index += 1; - if (accel_buffer_index >= MAX_ACCEL_BUFFER) { - accel_buffer_index = 0; - } -} - -void do_accel() { - /* Report the readings of the acceleration sensor. */ - int avg_x = 0; - int avg_y = 0; - int avg_z = 0; - int min_x = 676; - int min_y = 676; - int min_z = 676; - int max_x = 0; - int max_y = 0; - int max_z = 0; - for (unsigned char i = 0; i < MAX_ACCEL_BUFFER; i += 1) { - for (unsigned char axe = 0; axe < 3; ++axe) { - avg_x += accel_buffer[i][axe]; - min_x = min(min_x, accel_buffer[i][axe]); - max_x = max(max_x, accel_buffer[i][axe]); - } - } - avg_x /= MAX_ACCEL_BUFFER; - avg_y /= MAX_ACCEL_BUFFER; - avg_z /= MAX_ACCEL_BUFFER; - Serial.write(0xff); - Serial.write(map(avg_x, 0, 676, 0, 255)); - Serial.write(map(avg_y, 0, 676, 0, 255)); - Serial.write(map(avg_z, 0, 676, 0, 255)); - Serial.write(map(min_x, 0, 676, 0, 255)); - Serial.write(map(min_y, 0, 676, 0, 255)); - Serial.write(map(min_z, 0, 676, 0, 255)); - Serial.write(map(max_x, 0, 676, 0, 255)); - Serial.write(map(max_y, 0, 676, 0, 255)); - Serial.write(map(max_z, 0, 676, 0, 255)); -}
--- a/servo/battery.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,24 +0,0 @@ - -void check_battery() { - static int battery_fail = 0; - int reading = analogRead(A7); - if (reading < 762) { - /* At about 3.74V start worrying. */ - battery_fail += 1; - } else { - battery_fail = max(0, battery_fail - 1); - } - if (battery_fail > 10) { - /* If it doesn't improve for a second, beep. */ - tone(A3, 880, 100); - } - if (battery_fail > 40) { - /* If it stays bad for 4s, shut down. */ - fold(); - } -} - -void do_battery() { - Serial.write(0xff); - Serial.write(map(analogRead(A7), 0, 1023, 0, 255)); -}
--- a/servo/beep.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,17 +0,0 @@ - -void beep_setup() { - pinMode(A3, OUTPUT); - delay(50); - tone(A3, 440, 200); - delay(200); - tone(A3, 523, 100); - delay(150); - tone(A3, 880, 100); -} - -void do_beep() { - /* Emit a beep of specified frequency and duration. */ - int frequency = _serial_read_word(); - int duration = Serial.read(); - tone(A3, frequency, duration * 10); -}
--- a/servo/fastFunctions.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,129 +0,0 @@ -// const float PI = 3.141592; -const float PIby2 = PI/2; - - -const float FSIN_TABLE[91] = { -0.000000, 0.017452, 0.034900, 0.052336, 0.069757, 0.087156, 0.104529, 0.121869, 0.139173, 0.156435, 0.173648, 0.190809, 0.207912, 0.224951, 0.241922, 0.258819, 0.275637, 0.292372, 0.309017, 0.325568, 0.342020, 0.358368, 0.374607, 0.390731, 0.406737, 0.422618, 0.438371, 0.453991, 0.469472, 0.484810, -0.500000, 0.515038, 0.529919, 0.544639, 0.559193, 0.573576, 0.587785, 0.601815, 0.615662, 0.629320, 0.642788, 0.656059, 0.669131, 0.681998, 0.694658, 0.707107, 0.719340, 0.731354, 0.743145, 0.754710, 0.766044, 0.777146, 0.788011, 0.798636, 0.809017, 0.819152, 0.829038, 0.838671, 0.848048, 0.857167, -0.866025, 0.874620, 0.882948, 0.891007, 0.898794, 0.906308, 0.913546, 0.920505, 0.927184, 0.933580, 0.939693, 0.945519, 0.951057, 0.956305, 0.961262, 0.965926, 0.970296, 0.974370, 0.978148, 0.981627, 0.984808, 0.987688, 0.990268, 0.992546, 0.994522, 0.996195, 0.997564, 0.998630, 0.999391, 0.999848, 1.000000 -}; - -const float FACOS_TABLE[51] = { -0.643501, 0.636804, 0.630047, 0.623226, 0.616339, 0.609385, 0.602361, 0.595265, 0.588093, 0.580844, 0.573513, 0.566098, 0.558596, 0.551003, 0.543314, 0.535527, 0.527635, 0.519636, 0.511523, 0.503291, 0.494934, 0.486446, 0.477819, 0.469046, 0.460118, 0.451027, 0.441761, 0.432311, -0.422663, 0.412803, 0.402716, 0.392384, 0.381788, 0.370904, 0.359707, 0.348166, 0.336246, 0.323904, 0.311092, 0.297747, 0.283794, 0.26914, 0.253662, 0.237199, 0.21953, 0.200335, 0.179125, 0.155075, 0.126576, 0.089473, 0 - -}; - -int FloatToInt(float input){ - // this is an alternative to cast number directly, to avoid floor rounding - - int temp = (int)input; // floor-rounded input - float dif = input - (float)temp; // - if (dif < 0.5) return temp; - else return temp+1; -} - - -float fsin(int deg){ - // check for pos/neg sign - // get absolute value - int sign = 1; - if (deg<0){ - sign = -1; - deg = -deg; - } - // make sure degree under 360 - while (deg>=360) deg -= 360; - - if (deg<90) - return sign * FSIN_TABLE[deg]; - else if ((deg>=90) && (deg<180)) - return sign * FSIN_TABLE[180-deg]; - else if ((deg>=180) && (deg<270)) - return sign * -1*FSIN_TABLE[deg-180]; - else //if ((deg>=270) && (deg<360)) - return sign * -1*FSIN_TABLE[360-deg]; -} - -float fcos(int deg){ - // get absolute value (symeric) - if (deg<0) deg = -deg; - // make sure degree under 360 - while (deg>=360) deg -= 360; - - if (deg<90) - return FSIN_TABLE[90-deg]; - else if ((deg>=90) && (deg<180)) - return (-1*FSIN_TABLE[deg-90]); - else if ((deg>=180) && (deg<270)) - return (-1*FSIN_TABLE[270-deg]); - else// if ((deg>=270) && (deg<360)) - return FSIN_TABLE[deg-270]; -} - -float facos(float num){ - - // return 0 if input is out of range - if (num < -1 || num > 1) return 0; - - float ACOS = 0.0; - - // - if (num == -1.0) ACOS = PI; - else if (num == 0.0) ACOS = PIby2; - else if (num == 1.0) ACOS = 0; - else{ - - // if num<0.8, use polinomial - // if num>=0.8, use table - - if (abs(num) <=0.8){ - - float num2 = num*num; - float num3 = num2*num; - float num5 = num3*num2; - float num7 = num5*num2; - - ACOS = PIby2 - num - 0.166667*num3 - 0.075*num5 - 0.044642*num7; - - } - - else{ // abs(num) > 0.8 - - float findex = (abs(num)*1000.0 - 800.0)*0.25;// convert to index - int index = FloatToInt(findex); - - float prop = findex-(float)index; // proportion of first index - - ACOS = prop*FACOS_TABLE[index]+(1-prop)*FACOS_TABLE[index+1]; - - if (num < 0.0) - ACOS = PI - ACOS; - - } - } - - return ACOS; - -} - - -float fatan2(float a, float b){ - // atan(a/b) - - // reflected about origin (angle can only be -90 to 90) - - if (a==0){ - if (b>=0) return 0; - else return 0; // PI - PI - } - else if (a>0){ - if (b==0) return PIby2; - else return atan(a/b); - } - else { // a<0 - if (b==0) return -PIby2; - else return atan(a/b); - } - -}
--- a/servo/legs.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,27 +0,0 @@ -// Leg dimensions. -#define LEG_COXA 22.5 -#define LEG_FEMUR 38.0 -#define LEG_TIBIA 30.0 -#define LEG_BASE 15.0 - -const unsigned char LEG_HIP[4] = {11, 8, 9, 10}; -const unsigned char LEG_KNEE[4] = {4, 7, 6, 5}; -const unsigned char LEG_ANKLE[4] = {0, 3, 2, 1}; -const char LEG_X[4] = {-1, -1, 1, 1}; -const char LEG_Y[4] = {-1, 1, 1, -1}; - - -void fold() { - /* Folds the robots and shut downs all servos. Doesn't return. */ - for (int leg = 0; leg < 4; leg += 1) { - int d = LEG_X[leg] * LEG_X[leg]; - servo_degrees(LEG_HIP[leg], 90 + 45 * d); - servo_degrees(LEG_KNEE[leg], 90 - 90 * d); - servo_degrees(LEG_ANKLE[leg], 90 + 90 * d); - } - delay(250); - servo_shutdown(); - while (1) { - delay(1000); - } -}
--- a/servo/servo.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,76 +0,0 @@ -// Time of a signle frame, in milliseconds. -#define FRAME_TIME 10 - -// Speed of the serial port. -#define SERIAL_SPEED 115200 -// 115200 -// 38400 -// 9600 - -// Total number of servos. -#define MAX_SERVO 12 - -void do_serial_command() { - static int serial_command = 0; - /* Read and execute a command from the serial port's buffer. */ - switch (serial_command) { - case 0x00: - /* No command, read one. */ - if (Serial.available() < 1) { return; } - serial_command = Serial.read(); - return; - case 0xff: - /* Move a single servo by degrees. */ - if (Serial.available() < 2) { return; } - do_servo_degrees(); - break; - case 0xfe: - /* Move all servos by degrees. */ - if (Serial.available() < MAX_SERVO) { return; } - serial_command = 0; - do_servos_degrees(); - break; - case 0xfd: - /* Move a single servo by microseconds. */ - if (Serial.available() < 3) { return; } - do_servo_microseconds(); - break; - case 0xfc: - /* Move all servos by microseconds. */ - if (Serial.available() < MAX_SERVO * 2) { return; } - do_servos_microseconds(); - break; - case 0xf0: - /* Read the accelerometer sensor. */ - do_accel(); - break; - case 0xf1: - /* Beep. */ - if (Serial.available() < 3) { return; } - do_beep(); - break; - case 0xf2: - /* Read the battery voltage. */ - do_battery(); - break; - } - serial_command = 0; -} - -void setup() { - Serial.begin(SERIAL_SPEED); - servo_setup(); - beep_setup(); -} - -void loop() { - static unsigned long last_tick = 0; - unsigned long now = millis(); - - do_serial_command(); - if (now > last_tick + FRAME_TIME || now < last_tick) { - read_accel(); - check_battery(); - last_tick = now; - } -}
--- a/servo/servos.ino Sun Sep 14 13:12:28 2014 +0200 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,97 +0,0 @@ -#include <Servo.h> - - -// Range of the servos. -#define SERVO_FREQ_MIN 600 -#define SERVO_FREQ_MAX 2400 - -const unsigned char SERVO_PIN[MAX_SERVO] = { - 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13 -}; -const char SERVO_REVERSE[MAX_SERVO] = { - 1, -1, 1, -1, 1, -1, 1, -1, -1, 1, 1, -1 -}; -const char SERVO_TRIM[MAX_SERVO] = { - -50, 0, -50, -50, -70, 50, 30, 50, 0, 0, 0, 0 -}; -const double US_PER_DEG = double(SERVO_FREQ_MAX - SERVO_FREQ_MIN) / 180; - -Servo servos[MAX_SERVO]; - - -void servo_write(unsigned char servo, unsigned int us) { - int pos = min(max(us + SERVO_TRIM[servo], SERVO_FREQ_MIN), SERVO_FREQ_MAX); - if (SERVO_REVERSE[servo] < 0) { - pos = SERVO_FREQ_MAX + SERVO_FREQ_MIN - pos; - } - servos[servo].writeMicroseconds(pos); -} - -void servo_degrees(unsigned char servo, double angle) { - servo_write(servo, SERVO_FREQ_MIN + angle * US_PER_DEG); -} - -void servo_setup() { - for (int servo = MAX_SERVO - 1; servo >= 0; servo -= 1) { - /* First 2 pins are used for the serial. */ - servos[servo].attach(SERVO_PIN[servo], SERVO_FREQ_MIN, SERVO_FREQ_MAX); - servo_write(servo, 1500); - /* Don't set all servos at once, save power. */ - delay(150); - } -} - -void servo_shutdown() { - for (int servo = 0; servo < MAX_SERVO; servo += 1) { - servos[servo].detach(); - } -} - -int _serial_read_word() { - /* Read two bytes from the serial port and make an int from them. */ - int low; - int high; - low = Serial.read(); - high = Serial.read(); - return low | (high << 8); -} - - -void do_servo_degrees() { - /* Move a single servo by degrees. */ - int servo = Serial.read(); - int pos = Serial.read(); - if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) { - servos[servo].write(pos); - } -} - -void do_servos_degrees() { - /* Move all servos by degrees. */ - for (int servo = 0; servo < MAX_SERVO; servo += 1) { - int pos = Serial.read(); - if (pos >= 0 && pos <= 180) { - servos[servo].write(pos); - } - } -} - -void do_servo_microseconds() { - /* Move a single servo by microseconds. */ - int servo = Serial.read(); - int pos = _serial_read_word(); - if (servo >= 0 && servo < MAX_SERVO && - pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) { - servo_write(servo, pos); - } -} - -void do_servos_microseconds() { - /* Move all servos by microseconds. */ - for (int servo = 0; servo < MAX_SERVO; servo += 1) { - int pos = _serial_read_word(); - if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) { - servo_write(servo, pos); - } - } -}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/accel.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,53 @@ +// 3.3V maximum for the acceleration sensor. +#define ACCEL_MAX 676 + +// How many acceleration measurements to cache. +#define MAX_ACCEL_BUFFER 16 + + +int accel_buffer[MAX_ACCEL_BUFFER][3]; + + +void read_accel() { + static unsigned char accel_buffer_index = 0; + accel_buffer[accel_buffer_index][0] = analogRead(A2); + accel_buffer[accel_buffer_index][1] = analogRead(A1); + accel_buffer[accel_buffer_index][2] = analogRead(A0); + accel_buffer_index += 1; + if (accel_buffer_index >= MAX_ACCEL_BUFFER) { + accel_buffer_index = 0; + } +} + +void do_accel() { + /* Report the readings of the acceleration sensor. */ + int avg_x = 0; + int avg_y = 0; + int avg_z = 0; + int min_x = 676; + int min_y = 676; + int min_z = 676; + int max_x = 0; + int max_y = 0; + int max_z = 0; + for (unsigned char i = 0; i < MAX_ACCEL_BUFFER; i += 1) { + for (unsigned char axe = 0; axe < 3; ++axe) { + avg_x += accel_buffer[i][axe]; + min_x = min(min_x, accel_buffer[i][axe]); + max_x = max(max_x, accel_buffer[i][axe]); + } + } + avg_x /= MAX_ACCEL_BUFFER; + avg_y /= MAX_ACCEL_BUFFER; + avg_z /= MAX_ACCEL_BUFFER; + Serial.write(0xff); + Serial.write(map(avg_x, 0, 676, 0, 255)); + Serial.write(map(avg_y, 0, 676, 0, 255)); + Serial.write(map(avg_z, 0, 676, 0, 255)); + Serial.write(map(min_x, 0, 676, 0, 255)); + Serial.write(map(min_y, 0, 676, 0, 255)); + Serial.write(map(min_z, 0, 676, 0, 255)); + Serial.write(map(max_x, 0, 676, 0, 255)); + Serial.write(map(max_y, 0, 676, 0, 255)); + Serial.write(map(max_z, 0, 676, 0, 255)); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/battery.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,28 @@ + +void check_battery() { + static int battery_fail = 0; + int reading = analogRead(A7); + if (reading < 762) { + /* At about 3.74V start worrying. */ + battery_fail += 1; + } else { + battery_fail = max(0, battery_fail - 1); + } + if (battery_fail > 10) { + /* If it doesn't improve for a second, beep. */ + tone(A3, 880, 100); + } + if (battery_fail > 40) { + /* If it stays bad for 4s, shut down. */ + fold(); + } +} + +void do_battery() { + Serial.write(0xff); + Serial.write(map(analogRead(A7), 0, 1023, 0, 255)); +} + +void do_shutdown() { + fold(); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/beep.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,17 @@ + +void beep_setup() { + pinMode(A3, OUTPUT); + delay(50); + tone(A3, 440, 200); + delay(200); + tone(A3, 523, 100); + delay(150); + tone(A3, 880, 100); +} + +void do_beep() { + /* Emit a beep of specified frequency and duration. */ + int frequency = _serial_read_word(); + int duration = Serial.read(); + tone(A3, frequency, duration * 10); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/legs.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,27 @@ +// Leg dimensions. +#define LEG_COXA 22.5 +#define LEG_FEMUR 38.0 +#define LEG_TIBIA 30.0 +#define LEG_BASE 15.0 + +const unsigned char LEG_HIP[4] = {11, 8, 9, 10}; +const unsigned char LEG_KNEE[4] = {4, 7, 6, 5}; +const unsigned char LEG_ANKLE[4] = {0, 3, 2, 1}; +const char LEG_X[4] = {-1, -1, 1, 1}; +const char LEG_Y[4] = {-1, 1, 1, -1}; + + +void fold() { + /* Folds the robots and shut downs all servos. Doesn't return. */ + for (int leg = 0; leg < 4; leg += 1) { + int d = LEG_X[leg] * LEG_Y[leg]; + servo_degrees(LEG_HIP[leg], 90 + 45 * d); + servo_degrees(LEG_KNEE[leg], 90 - 90 * d); + servo_degrees(LEG_ANKLE[leg], 90 + 90 * d); + } + delay(250); + servo_shutdown(); + while (1) { + delay(1000); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/servos.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,97 @@ +#include <Servo.h> + + +// Range of the servos. +#define SERVO_FREQ_MIN 600 +#define SERVO_FREQ_MAX 2400 + +const unsigned char SERVO_PIN[MAX_SERVO] = { + 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13 +}; +const char SERVO_REVERSE[MAX_SERVO] = { + 1, -1, 1, -1, 1, -1, 1, -1, -1, 1, 1, -1 +}; +const char SERVO_TRIM[MAX_SERVO] = { + -50, 0, -50, -50, -70, 50, 30, 50, 0, 0, 0, 0 +}; +const double US_PER_DEG = double(SERVO_FREQ_MAX - SERVO_FREQ_MIN) / 180; + +Servo servos[MAX_SERVO]; + + +void servo_write(unsigned char servo, unsigned int us) { + int pos = min(max(us + SERVO_TRIM[servo], SERVO_FREQ_MIN), SERVO_FREQ_MAX); + if (SERVO_REVERSE[servo] < 0) { + pos = SERVO_FREQ_MAX + SERVO_FREQ_MIN - pos; + } + servos[servo].writeMicroseconds(pos); +} + +void servo_degrees(unsigned char servo, double angle) { + servo_write(servo, SERVO_FREQ_MIN + angle * US_PER_DEG); +} + +void servo_setup() { + for (int servo = MAX_SERVO - 1; servo >= 0; servo -= 1) { + /* First 2 pins are used for the serial. */ + servos[servo].attach(SERVO_PIN[servo], SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_write(servo, 1500); + /* Don't set all servos at once, save power. */ + delay(150); + } +} + +void servo_shutdown() { + for (int servo = 0; servo < MAX_SERVO; servo += 1) { + servos[servo].detach(); + } +} + +int _serial_read_word() { + /* Read two bytes from the serial port and make an int from them. */ + int low; + int high; + low = Serial.read(); + high = Serial.read(); + return low | (high << 8); +} + + +void do_servo_degrees() { + /* Move a single servo by degrees. */ + int servo = Serial.read(); + int pos = Serial.read(); + if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) { + servos[servo].write(pos); + } +} + +void do_servos_degrees() { + /* Move all servos by degrees. */ + for (int servo = 0; servo < MAX_SERVO; servo += 1) { + int pos = Serial.read(); + if (pos >= 0 && pos <= 180) { + servos[servo].write(pos); + } + } +} + +void do_servo_microseconds() { + /* Move a single servo by microseconds. */ + int servo = Serial.read(); + int pos = _serial_read_word(); + if (servo >= 0 && servo < MAX_SERVO && + pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) { + servo_write(servo, pos); + } +} + +void do_servos_microseconds() { + /* Move all servos by microseconds. */ + for (int servo = 0; servo < MAX_SERVO; servo += 1) { + int pos = _serial_read_word(); + if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) { + servo_write(servo, pos); + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ukubik/ukubik.ino Mon Sep 15 20:05:00 2014 +0200 @@ -0,0 +1,80 @@ +// Time of a signle frame, in milliseconds. +#define FRAME_TIME 10 + +// Speed of the serial port. +#define SERIAL_SPEED 115200 +// 115200 +// 38400 +// 9600 + +// Total number of servos. +#define MAX_SERVO 12 + +void do_serial_command() { + static int serial_command = 0; + /* Read and execute a command from the serial port's buffer. */ + switch (serial_command) { + case 0x00: + /* No command, read one. */ + if (Serial.available() < 1) { return; } + serial_command = Serial.read(); + return; + case 0xff: + /* Move a single servo by degrees. */ + if (Serial.available() < 2) { return; } + do_servo_degrees(); + break; + case 0xfe: + /* Move all servos by degrees. */ + if (Serial.available() < MAX_SERVO) { return; } + serial_command = 0; + do_servos_degrees(); + break; + case 0xfd: + /* Move a single servo by microseconds. */ + if (Serial.available() < 3) { return; } + do_servo_microseconds(); + break; + case 0xfc: + /* Move all servos by microseconds. */ + if (Serial.available() < MAX_SERVO * 2) { return; } + do_servos_microseconds(); + break; + case 0xf0: + /* Read the accelerometer sensor. */ + do_accel(); + break; + case 0xf1: + /* Beep. */ + if (Serial.available() < 3) { return; } + do_beep(); + break; + case 0xf2: + /* Read the battery voltage. */ + do_battery(); + break; + case 0xf3: + /* Fold up and shut down the robot. */ + do_shutdown(); + break; + } + serial_command = 0; +} + +void setup() { + Serial.begin(SERIAL_SPEED); + servo_setup(); + beep_setup(); +} + +void loop() { + static unsigned long last_tick = 0; + unsigned long now = millis(); + + do_serial_command(); + if (now > last_tick + FRAME_TIME || now < last_tick) { + read_accel(); + check_battery(); + last_tick = now; + } +}