Hi, I'm a beginner in Arduino.
I try to rewrite Rapiro code , and add some functions like controlling the angle of motors independently. But,I failed and don't know to do that...hope someone is can help me with this code.Any help is appreciated!
#include <Servo.h>
#define POWER 17 // Servo power supply control pin
#define SHIFT 7
#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 ERR -1 // Error
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';
int i = 0;
int t = 1;
Servo servo[MAXSN];
int trim[MAXSN] = { 0, // Head yaw
0, // Waist yaw
0, // 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
0}; // 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};
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(7); // Head yaw
servo[1].attach(9); // Waist yaw
servo[2].attach(10); // R Sholder roll
servo[3].attach(8); // R Sholder pitch
servo[4].attach(11); // 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
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]);
}
Serial.begin(115200); //port
Serial.print("Ready");
pinMode(POWER, OUTPUT);
digitalWrite(POWER, HIGH);
Serial.flush();
}
void loop()
{
if (Serial.available())
{
static int v=0;
char ch = Serial.read();
switch(ch)
{
case '0'...'9': //angle function
v = v*10 + ch - '0';
break;
case 'a': //control servo[0]
servo[0].write(v); //set angle
v = 0;
break;
case 'b': //control servo[1]
servo[1].write(v);
v = 0;
delay(50);
break;
case 'c': //control servo[2]
servo[2].write(v);
v = 0;
break;
case '!':
motionNumber = 0;
mode = 'M';
digitalWrite(POWER, HIGH);
Serial.print("Motion");
Serial.print(motionNumber);
break;
case '@':
motionNumber = 1;
mode = 'M';
digitalWrite(POWER, HIGH);
Serial.print("Motion");
Serial.print(motionNumber);
break;
case '#':
motionNumber = 9;
mode = 'M';
digitalWrite(POWER, HIGH);
Serial.print("Motion");
Serial.print(motionNumber);
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]);
}
}
else if(mode == 'M') {
nextFrame();
}
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];
}
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);
}
} else {
for(i = 0; i < MAXSN; i++) {
deltaAngle[i] = 0;
}
}
startTime = millis();
endTime = startTime + (bufferTime * 100);
bufferTime = 0;
}