如何改寫馬達的控制方式...?
各位好，我是剛接觸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;
int trim = { 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 = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};// Initialize array to 0
int targetAngle = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};// Initialize array to 0
int deltaAngle = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};// Initialize array to 0
uint8_t bufferAngle = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};// Initialize array to 0
uint8_t tempAngle = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t motion={
{// 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.attach(7); // Head yaw
servo.attach(9); // Waist yaw
servo.attach(10); // R Sholder roll
servo.attach(8); // R Sholder pitch
servo.attach(11); // R Hand grip
servo.attach(12); // L Sholder roll
servo.attach(13); // L Sholder pitch
servo.attach(14); // L Hand grip
servo.attach(4); // R Foot yaw
servo.attach(2); // R Foot pitch
servo.attach(15);// L Foot yaw
servo.attach(16);// L Foot pitch
for( i = 0; i < MAXSN; i++) {
targetAngle = motion << SHIFT;
nowAngle = targetAngle;
servo.write((nowAngle >> SHIFT) + trim);
}
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 (問題點)
servo.write(v);//set angle
v = 0;
break;
case 'b': //control servo (問題點)
servo.write(v);
v = 0;
delay(50);
break;
case 'c': //control servo (問題點)
servo.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 = targetAngle - (deltaAngle * remainingTime);
servo.write((nowAngle >> SHIFT) + trim);
}
}
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 = motion;
}
bufferTime = motion;
nextPose();
}
//Make a pose
int nextPose() {
if(bufferTime > 0) {
for(i = 0; i < MAXSN; i++) {
targetAngle = bufferAngle << SHIFT;
deltaAngle = ((bufferAngle << SHIFT) - nowAngle) / (bufferTime * 10);
}
} else {
for(i = 0; i < MAXSN; i++) {
deltaAngle = 0;
}
}
startTime = millis();
endTime = startTime + (bufferTime * 100);
bufferTime = 0;
}
rapiro的代码，你可以参考完整的代码，里面可以实现单独的servo控制。另外，请在发帖时如果有代码可以使用代码模式。
页:
[1]