servo speed selector

經過幾日測試都想不到如何可以用 program 計出改變了的角度 ,
現在以下 program 紅色一段文字 , 是一個程式 , 可除着 servo 改變角度而自動 set 出 servo 的速度 ,
己証實是可行 , 但因為 deg_a 及 deg_b 是沒有 store 入 location 內的 , 所以現正設法如何可將 ,
location 內的角度名為 deg_a 及 deg_b , 如果成功 , 以後可以不用人手計角度 ,
希望做到程式 function :
deg_a 減 deg_b ,
如果減後角度大於 150 度 , set speed 5
如果減後角度大於 130 度 , set speed 4
如果減後角度大於 100 度 , set speed 3
如果減後角度大於 80 度 , set speed 2
如果減後角度大於 60 度 , set speed 1
否則 set speed 0
但這組 deg_a 及 deg_b 一定要代表了 location store 其中 servo 的角度 , 唔知點寫 ???
P.S. Dr. Victor 明唔明我想點寫 ? 以下是成功了一半的 program , 但現在唔知點寫 deg_a 及 deg_b 入 location ,
請問做唔做到 ??
測試程式如下 :
// updown with speed control
// Action: Standup than Sit to Standup
// by BloodKeith 20 Oct 2007
#define HEAD 0
#define NECK 14
#define L_ONE 1
#define L_TWO 2
#define L_THREE 3
#define L_FOUR 4
#define L_FIVE 5
#define L_SIX 6
#define WAIST 7
#define R_ONE 20
#define R_TWO 19
#define R_THREE 18
#define R_FOUR 17
#define R_FIVE 16
#define R_SIX 15
main()
{
int   i , deg_a , deg_b , t_deg;

sys(0, 1);     // select COM port 1
setcomm(9600);
delay(2000);

deg_a=20 ;                                 //servo head start degree
deg_b=160 ;                                //servo end degree
t_deg=deg_a-deg_b;                         //start degree – end degree
if( t_deg > -90 ) set_servo_speed(i, 1);   //if sum > -90 degree , set speed to 1
if( t_deg <-80 ) set_servo_speed(i, 1);    //if sum <-80 degree , set speed to 1
if( t_deg == -86 ) set_servo_speed(i, 1);  //if sum = -86 degree , set speed to 1
else set_servo_speed(i, 5);                //set servo speed 5

for(i=L_ONE; i<=L_ONE; i++) set_servo_speed(i, 5);
for(i=L_TWO; i<=L_TWO; i++) set_servo_speed(i, 5);
for(i=L_THREE; i<=L_THREE; i++) set_servo_speed(i, 5);
for(i=L_FOUR; i<=L_FOUR; i++) set_servo_speed(i, 5);
for(i=L_FIVE; i<=L_FIVE; i++) set_servo_speed(i, 5);
for(i=L_SIX; i<=L_SIX; i++) set_servo_speed(i, 5);
for(i=WAIST; i<=WAIST; i++) set_servo_speed(i, 0);
for(i=NECK; i<=NECK; i++) set_servo_speed(i, 0);
for(i=R_SIX; i<=R_SIX; i++) set_servo_speed(i, 5);
for(i=R_FIVE; i<=R_FIVE; i++) set_servo_speed(i, 5);
for(i=R_FOUR; i<=R_FOUR; i++) set_servo_speed(i, 5);
for(i=R_THREE; i<=R_THREE; i++) set_servo_speed(i, 5);
for(i=R_TWO; i<=R_TWO; i++) set_servo_speed(i, 5);
for(i=R_ONE; i<=R_ONE; i++) set_servo_speed(i, 5);

// Store data to mem locs
stand_still(); writec(232, 252, 0); writec(232, 0, 0); delay(1000); // Loc 0
up_Step_nine(); writec(232, 252, 0); writec(232, 19, 0); delay(1000); // Loc 19
// retrieve data from mem locs
loop {
writec(232, 253, 0); writec(232, 0, 0); delay(250); toggle(5); // Loc 0
writec(232, HEAD,  50); writec(232, deg_a, 50); delay(1000); // Servo 0
writec(232, HEAD,  50); writec(232, deg_b, 50); delay(1000); // Servo 0
writec(232, NECK,  50); writec(232, 73, 50); delay(1000); // Servo 0
writec(232, NECK,  50); writec(232, 111, 50); delay(1000); // Servo 0
writec(232, 253, 0); writec(232, 19, 0); delay(500); toggle(3);toggle(5); // Loc 19
writec(232, WAIST,  50); writec(232, 116, 50); delay(1000); // Servo 7
writec(232, WAIST,  50); writec(232, 93, 50); delay(1000); // Servo 7
writec(232, WAIST,  50); writec(232, 80, 50); delay(1000); // Servo 7
writec(232, WAIST,  50); writec(232, 93, 50); delay(3000); // Servo 7
toggle(3);
}
}
set_servo_speed(svr, spd)
{
writec(232, 250, 0);
writec(232, svr, 0);
writec(232, spd, 0);
}
stand_still()
{
writec(232, HEAD,  50); writec(232, 95, 50); // Servo 0
writec(232, L_ONE,  50); writec(232, 89, 50); // Servo 1
writec(232, L_TWO,  50); writec(232, 97, 50); // Servo 2
writec(232, L_THREE,  25); writec(232, 90, 50); // Servo 3
writec(232, L_FOUR,  50); writec(232, 90, 50); // Servo 4
writec(232, L_FIVE,  50); writec(232, 138, 50); // Servo 5
writec(232, L_SIX,  50); writec(232, 80, 50); // Servo 6
writec(232, WAIST,  50); writec(232, 93, 50); // Servo 7
writec(232, NECK, 50); writec(232, 96, 50); // Servo 14
writec(232, R_SIX, 50); writec(232, 98, 50); // Servo 15
writec(232, R_FIVE, 50); writec(232, 41, 50); // Servo 16
writec(232, R_FOUR, 50); writec(232, 89, 50); // Servo 17
writec(232, R_THREE, 50); writec(232, 90, 50); // Servo 18
writec(232, R_TWO, 50); writec(232, 90, 50); // Servo 19
writec(232, R_ONE, 50); writec(232, 91, 50); // Servo 20
}
up_Step_nine()
{
writec(232, HEAD,  50); writec(232, 95, 50); // Servo 0
writec(232, L_ONE,  50); writec(232, 68, 50); // Servo 1
writec(232, L_TWO,  50); writec(232, 97, 50); // Servo 2
writec(232, L_THREE,  50); writec(232, 106, 50); // Servo 3
writec(232, L_FOUR,  50); writec(232, 43, 50); // Servo 4
writec(232, L_FIVE,  50); writec(232, 78, 50); // Servo 5
writec(232, L_SIX,  50); writec(232, 80, 50); // Servo 6
writec(232, WAIST,  50); writec(232, 93, 50); // Servo 7
writec(232, NECK, 50); writec(232, 96, 50); // Servo 14
writec(232, R_SIX, 50); writec(232, 98, 50); // Servo 15
writec(232, R_FIVE, 50); writec(232, 101, 50); // Servo 16
writec(232, R_FOUR, 50); writec(232, 136, 50); // Servo 17
writec(232, R_THREE, 50); writec(232, 74, 50); // Servo 18
writec(232, R_TWO, 50); writec(232, 90, 50); // Servo 19
writec(232, R_ONE, 50); writec(232, 112, 50); // Servo 20
}
廣告
  1. Victor
    十月 24, 2007 at 7:48 上午

    Keith,You can use command 245 to get the servo position.Please see the modified program: http://www.mcu.hk/tmp/keith-01.prg

  2. iRobot
    十月 25, 2007 at 10:41 上午

    你的意思是由 deg_a 至 deg_b 期間一直用同一個速度前進,還是會在不斷迫近目標值時漸漸減慢前進速度呢?

  3. BloodKeith
    十月 25, 2007 at 3:37 下午

    Thanks Victor , 我會試下先 ,
    I-robot , 我意思是保持同一速度 , 如果要做漸變速 , 個 combination 可能有成百個以上 , 對我來講實在太難了

  4. iRobot
    十月 26, 2007 at 3:08 下午

    啊原來是這意思,不過若有漸變速也不錯啊。

  5. BloodKeith
    十月 26, 2007 at 3:56 下午

    iRobot , 有漸變速當然好啦 , 看過 R-blue 其中一段片應是用了漸變速 ,
    段片係好似原地踏步 , 跟住漸收細步伐 , 最後停低 , 原來要做呢個動作一 D 都唔易 .

  6. BloodKeith
    十月 27, 2007 at 3:19 下午

    Hello coolwyc, 你架智能小車也很捧, 很多謝你的賞識

  1. No trackbacks yet.

發表迴響

在下方填入你的資料或按右方圖示以社群網站登入:

WordPress.com Logo

您的留言將使用 WordPress.com 帳號。 登出 / 變更 )

Twitter picture

您的留言將使用 Twitter 帳號。 登出 / 變更 )

Facebook照片

您的留言將使用 Facebook 帳號。 登出 / 變更 )

Google+ photo

您的留言將使用 Google+ 帳號。 登出 / 變更 )

連結到 %s

%d 位部落客按了讚: