Skip to content

Instantly share code, notes, and snippets.

@umhan35

umhan35/rapiro.c

Last active August 29, 2015 14:00
Show Gist options
  • Save umhan35/a1bdca101521f9f1dfa6 to your computer and use it in GitHub Desktop.
Save umhan35/a1bdca101521f9f1dfa6 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
rapiro.c
#include <Servo.h>
#define SHIFT 7
#define R 0 // Red LED
#define G 1 // Green LED
#define B 2 // Blue LED
#define TIME 15 // Column of Time
#define MAXSN 12 // Max Number of Servos
#define MAXMN 10 // Max Number of Motions
#define MAXFN 8 // Max Number of Frames
#define POWER 17 // Servo power supply control pin
#define ERR -1 // Error
int i = 0;
int t = 1;
Servo servo[MAXSN];
uint8_t eyes[3] = { 0, 0, 0};
// Fine angle adjustments (degrees)
int trim[MAXSN] = { 9, // Head yaw
-3, // Waist yaw
16, // R Sholder roll
0, // R Sholder pitch
0, // R Hand grip
0, // L Sholder roll
0, // L Sholder pitch
0, // L Hand grip
0, // R Foot yaw
0, // R Foot pitch
0, // L Foot yaw
-4}; // L Foot pitch
int nowAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int targetAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int deltaAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
uint8_t bufferAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
uint8_t tempAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int nowBright[3] = { 0, 0, 0}; // Initialize array to 0
int targetBright[3] = { 0, 0, 0}; // Initialize array to 0
int deltaBright[3] = { 0, 0, 0}; // Initialize array to 0
uint8_t bufferBright[3] = { 0, 0, 0}; // Initialize array to 0
uint8_t tempBright[3] = { 0, 0, 0}; // Initialize array to 0
double startTime = 0; // Motion start time(msec)
double endTime = 0; // Motion end time(msec)
int remainingTime = 0; // Motion remaining time(msec)
uint8_t bufferTime = 0; // Motion buffer time (0.1sec)
uint8_t motionNumber = 0;
uint8_t frameNumber = 0;
char mode = 'M';
uint8_t motion[MAXMN][MAXFN][16]={
{ // 0 Stop
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0,255, 10},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 1 Forward
{ 90, 90, 0, 90, 90,180, 90, 90, 80,110, 80,120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 70, 90, 70, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 70, 70, 70, 80, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,100, 60,100, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,110, 90,110, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,110,100,110,110, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 2 Back
{ 90, 90, 0, 90, 90,180, 90, 90,100,110,100,120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,110, 90,110, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,110, 70,110, 80, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 80, 30, 80, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 70, 90, 70, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 70,100, 70,110, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 3 Right
{ 90, 90, 0, 90, 90,180, 90, 90, 95,110, 85,120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,100, 90, 80, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,100, 70, 80, 80, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 85, 60, 95, 70, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 80, 90,100, 90, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 80,100,100,110, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 4 Left
{ 90, 90, 0, 90, 90,180, 90, 90, 95, 60, 85, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,100, 90, 80, 90, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90,100,100, 80,110, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 85,110, 95,120, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 80, 90,100, 90, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 80, 70,100, 80, 0, 0,255, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 5 Green
{ 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0, 0, 0, 10},
{100, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0,255, 0, 5},
{ 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0,255, 0, 5},
{ 80, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 0, 5},
{ 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0,255, 0, 10},
{ 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0,255, 0, 5},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 6 Yellow
{ 90,120,120,130, 90,180, 90, 90, 90, 90, 90, 90,255,255, 0, 7},
{ 90,120,120, 90, 90,180, 90, 90, 90, 90, 90, 90,255,255, 0, 7},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 7 Blue
{ 90, 90,120,130, 70, 60, 50,110, 90, 90, 90, 90, 0, 0,255, 10},
{ 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0, 0,255, 5},
{ 90, 90,120,130, 70, 60, 50,110, 90, 90, 90, 90, 0, 0,255, 5},
{ 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0, 0,255, 5},
{ 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0, 0,255, 15},
{ 90, 90, 90,130,110, 90, 50, 70, 90, 90, 90, 90, 0, 0,255, 3},
{ 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90, 0, 0,255, 3},
{ 90, 90, 90,130,110, 90, 50, 70, 90, 90, 90, 90, 0, 0,255, 3}
},
{ // 8 Red
{ 90, 60, 0, 90, 90, 60, 50, 90, 90, 90, 90, 90,255, 0, 0, 7},
{ 90, 60, 0, 90, 90, 60, 90, 90, 90, 90, 90, 90,255, 0, 0, 7},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 9 Push
{ 90, 90, 90,130,110,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 10},
{ 90, 90, 90,130,110,180, 50, 90, 90, 90, 90, 90, 0, 0,255, 5},
{ 90, 90, 90,130,110,180, 50, 90, 90, 90, 90, 90, 0, 0,255, 25},
{ 90, 90, 90,130, 90,180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 5},
{ 40,140, 90, 70, 90,180, 90, 90, 90, 90, 90, 90, 0, 0,255, 10},
{ 40,140, 90, 70, 90,180, 90, 90, 90, 90, 90, 90, 0, 0,255, 25},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90,180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
}
};
void setup() {
servo[0].attach(10); // Head yaw
servo[1].attach(11); // Waist yaw
servo[2].attach(9); // R Sholder roll
servo[3].attach(8); // R Sholder pitch
servo[4].attach(7); // R Hand grip
servo[5].attach(12); // L Sholder roll
servo[6].attach(13); // L Sholder pitch
servo[7].attach(14); // L Hand grip
servo[8].attach(4); // R Foot yaw
servo[9].attach(2); // R Foot pitch
servo[10].attach(15); // L Foot yaw
servo[11].attach(16); // L Foot pitch
eyes[R] = 6; // Red LED of eyes
eyes[G] = 5; // Green LED of eyes
eyes[B] = 3; // Blue LED of eyes
for( i = 0; i < MAXSN; i++) {
targetAngle[i] = motion[0][0][i] << SHIFT;
nowAngle[i] = targetAngle[i];
servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
}
for(i = 0; i < 3; i++) {
targetBright[i] = 0 << SHIFT;
nowBright[i] = targetBright[i];
analogWrite(eyes[i], nowBright[i] >> SHIFT);
}
Serial.begin(57600);
delay(500);
pinMode(POWER, OUTPUT);
digitalWrite(POWER, HIGH);
}
void loop() {
int buf = ERR;
if(Serial.available()) {
if(Serial.read() == '#') {
while(!Serial.available()){}
switch(Serial.read()) {
case 'M':
buf = readOneDigit();
if(buf != ERR){
motionNumber = buf;
mode = 'M';
digitalWrite(POWER, HIGH);
Serial.print("#M");
Serial.print(motionNumber);
} else {
Serial.print("#EM");
}
break;
case 'P':
buf = getPose();
if(buf != ERR) {
mode = 'P';
digitalWrite(POWER, HIGH);
Serial.print("#PT");
printThreeDigit(buf);
} else {
Serial.print("#EP");
}
break;
case 'Q':
Serial.print("#Q");
if(mode == 'M') {
Serial.print("M");
Serial.print(motionNumber);
Serial.print("T");
buf = (endTime-millis()) /100;
if(buf < 0) { buf = 0;}
printThreeDigit(buf);
}
if(mode == 'P') {
Serial.print("PT");
buf = (endTime-millis()) /100;
if(buf < 0) { buf = 0;}
printThreeDigit(buf);
}
break;
case 'C':
Serial.print("#C");
if(bufferTime > 0) {
Serial.print("F");
} else {
Serial.print("0");
}
break;
default:
Serial.print("#E");
break;
}
}
}
if(endTime > millis()) {
remainingTime = (endTime - millis()) / 10;
for( i = 0; i < MAXSN; i++) {
nowAngle[i] = targetAngle[i] - (deltaAngle[i] * remainingTime);
servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
}
for( i = 0; i < 3; i++) {
nowBright[i] = targetBright[i] - (deltaBright[i] * remainingTime);
analogWrite(eyes[i], nowBright[i] >> SHIFT);
}
} else if(mode == 'M') {
nextFrame();
} else if(mode == 'P') {
if(bufferTime > 0){
nextPose();
} else if(endTime + 500 < millis()){
//digitalWrite(POWER, LOW);
}
}
}
//Motion Play
void nextFrame() {
frameNumber++;
if(frameNumber >= MAXFN) {
frameNumber = 0;
}
for(i = 0; i < MAXSN; i++) {
bufferAngle[i] = motion[motionNumber][frameNumber][i];
}
for( i = 0; i < 3; i++) {
bufferBright[i] = motion[motionNumber][frameNumber][MAXSN+i];
}
bufferTime = motion[motionNumber][frameNumber][TIME];
nextPose();
}
//Make a pose
int nextPose() {
if(bufferTime > 0) {
for(i = 0; i < MAXSN; i++) {
targetAngle[i] = bufferAngle[i] << SHIFT;
deltaAngle[i] = ((bufferAngle[i] << SHIFT) - nowAngle[i]) / (bufferTime * 10);
}
for( i = 0; i < 3; i++) {
targetBright[i] = bufferBright[i] << SHIFT;
deltaBright[i] = ((bufferBright[i] << SHIFT) - nowBright[i]) / (bufferTime * 10);
}
} else {
for(i = 0; i < MAXSN; i++) {
deltaAngle[i] = 0;
}
for(i = 0; i < 3; i++) {
deltaBright[i] = 0;
}
}
startTime = millis();
endTime = startTime + (bufferTime * 100);
bufferTime = 0;
}
//get buffer values of the next pose from serial data
int getPose() {
int buf = 0;
int value = 0;
int maximum = 255;
boolean readPose = true;
if(bufferTime == 0) {
//Initialize array to target angle
for(i = 0; i < MAXSN; i++) {
tempAngle[i] = bufferAngle[i];
}
for( i = 0; i < 3; i++) {
tempBright[i] = bufferBright[i];
}
} else {
buf = ERR;
readPose = false;
}
//Read data
while(readPose) {
while(!Serial.available()) {}
switch(Serial.read()) {
case 'S':
buf = readOneDigit();
if(buf != ERR) {
value = buf *10;
buf = readOneDigit();
if(buf != ERR) {
value += buf;
if(0 <= value && value < MAXSN) {
while(!Serial.available()) {}
if(Serial.read() == 'A') {
maximum = 180;
buf = readThreeDigit(maximum);
if(buf != ERR) {
tempAngle[value] = buf;
} else {
readPose = false;
}
} else {
buf = ERR;
readPose = false;
}
} else {
buf = ERR;
readPose = false;
}
}
}
break;
case 'R':
maximum = 255;
buf = readThreeDigit(maximum);
if(buf != ERR) {
tempBright[R] = buf;
} else {
readPose = false;
}
break;
case 'G':
maximum = 255;
buf = readThreeDigit(maximum);
if(buf != ERR) {
tempBright[G] = buf;
} else {
readPose = false;
}
break;
case 'B':
maximum = 255;
buf = readThreeDigit(maximum);
if(buf != ERR) {
tempBright[B] = buf;
} else {
readPose = false;
}
break;
case 'T':
maximum = 255;
buf = readThreeDigit(maximum);
if(buf > 0) {
bufferTime = buf;
for(i = 0; i < MAXSN; i++){
bufferAngle[i] = tempAngle[i];
}
for( i = 0; i < 3; i++) {
bufferBright[i] = tempBright[i];
}
}
readPose = false;
break;
default:
buf = ERR;
readPose = false;
break;
}
}
return buf;
}
int printThreeDigit(int buf) {
String s = String(buf);
if(s.length() == 2){
Serial.print("0");
} else if (s.length() == 1) {
Serial.print("00");
}
Serial.print(s);
}
int digit;
//Read ASCII Three-digit
int readThreeDigit(int maximum) {
int buf;
buf = readOneDigit();
if(buf != ERR) {
digit = buf * 100;
buf = readOneDigit();
if(buf != ERR) {
digit += buf * 10;
buf = readOneDigit();
if(buf != ERR) {
digit += buf;
if(digit <= maximum) {
buf = digit;
} else {
buf = ERR;
}
}
}
}
return buf;
}
//Read ASCII One-digit
int readOneDigit() {
int buf;
while(!Serial.available()) {}
buf = Serial.read() - 48;
if(buf < 0 || 9 < buf){
buf = ERR;
}
return buf;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment