|
|
各位好，我是剛接觸arduino約一年多的菜鳥，最近試著改寫機器人控制的程式，卻遇到不會的地方。
我想要在程式裡添加可以直接控制任一顆馬達的角度的case，但試過幾種寫法仍然沒辦法達到，該怎麼改才會對呢?
#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
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 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 = 2;
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;
}
|
|
发表于 2016-1-13 19:47:07
