2015年12月30日 星期三

2016 準備夯桌遊

http://4rdp.blogspot.tw/2015/12/2016.html



桌遊是我好久沒去碰觸的玩意,它涉及我童年的回憶,輔助我數學的學習,培養我研發的能力,有興趣的讀者可以參閱舊文下棋學策略 做棋學研發

今天之所以開始拿筆寫這系列文章,是觀察到最近社會脈動發生一些質變趨勢:翻轉教育、自造者運動、兒童程式設計學習、桌遊店如雨後春筍般開店等。個人以過來人的經歷描述這對後代的影響,觀點不敢說百分之百完全正確,但八九不離十。

翻轉教育(Flipped Education)─ 是現在台灣不得不走的一條路,自從教育部推行一堆政策,如廣設大學、多元入學、十二年國教,雖想減少大考壓力,但將以往技職教育基石摧毀,加上大陸經濟崛起、製造產業外移、或轉為服務業、薪資不漲甚至低薪、少子化趨勢,另外,現代社會也資訊數位化,加上智慧手機普及,大人忙於工作,小朋友超愛玩電動玩具,不愛閱讀之類靜態休閒,如果又不加控制使用 3C 時間,那就會沉迷線上遊戲,對於學習這件事更缺乏努力的動機,因此台大電機系副教授葉丙成發起翻轉教育運動,希望提振學生自學動機。

自造者運動(Maker)─ 因為近代科技發達 3DP 與雷射切割機逐漸普及,遇上網路知識散播,還有 Arduino、Raspberry Pi 等開源電子神器加持讓以前許多 DIY 做不出來的作品,變得非常容易創作設計,也讓越多非電子專業玩家輕鬆進入玩具機器人、物聯網控制、四軸飛行器等自造領域嘗試創作,會投入自造者運動的夥伴們,都有些主動自學特質,有發明家的精神,想動手解決自身生活不便的問題。

兒童程式設計學習 ─ 如 Scratch、APP Inventor 免費好用適合程式初學者的開源軟體普及,歐美國家也順勢推行學齡兒童學習程式設計,二十一世紀的工業革命必定有機器人的要素,而設計機器人需要有程式設計的基礎,雖然程式設計或者電腦操作是本世紀必要的基本工作技能,不過是不是需要從小學生一年級就開始學?雖然我平日工作就是寫程式,但對這點個人抱持懷疑態度?稍後再論述觀點。

桌遊店方興未艾 ─ 只要 Google 一下「桌遊店」就會發現很多搜尋結果,甚至我的公司也出現桌遊社!每個人的童年一定有玩過桌遊經驗,無論輸贏,玩過一次只要有時間有機會一定還會想再玩一次,這點跟電玩遊戲很像,不過少了打打殺殺直覺反應式鍵盤操作,反多了需要棋奕深層策略思考,這對數學學習很有幫助,另外,一些桌遊與社會生活有些關聯,也可以幫助小朋友透過遊戲學習社會知識。桌遊店客源可能有兩類,一是 3 ~ 12 歲的親子家庭,如果這些店結合餐飲,大人吃東西聊天時,小朋友有東西玩不無聊,二為年輕族群,新奇好玩聚一起,不過因價位、翻桌率都不高,當客人新鮮感消失不再上門後,不知道這些桌遊店可以經營多久?會不會像以前保齡球館流行時一家家開,而退流行時一家一家關,有待觀察。

從這些趨勢來看,在翻轉教學與資訊教育方面,想培養創造力與提升學習動機,我比較推薦

2015年12月26日 星期六

第二代 P&B 機器蜘蛛

http://4rdp.blogspot.tw/2015/12/p.html

正好要組裝第二代 P&B 機器蜘蛛,因此拍照將機器蜘蛛主要零件秀出來,組裝過程也說明一下。

圖左下方有七個零件,它們剛好可以組出 6 x 2 或 4 x 2 機器蜘蛛一隻腳,圖右下方則是機器蜘蛛本體平台,中間的小孔是可以螺絲鎖付 Arduino、Raspberry Pi 之類控制板,雙層機器蜘蛛設計,最上層還可以架上 meArm 之類機器手臂。

控制器採用標準 Arduino Uno 及相配的傳感器擴充板,每隻腳 SG90 伺服馬達用 2 顆,雙孔 USB 行動電源一個,因為馬達相當耗電,所以需要能輸出 5V 2.1A,紅外線遙控器及接收器各一,除了機器蜘蛛本體,其餘估計這些電子套件兩千多元可以在電子材料行購得。


2015年12月23日 星期三

訓練數學感 83 ─ 增額保單

http://4rdp.blogspot.tw/2015/12/83.html

小明的保險經紀人向他推薦增額保單理財方式,左表是二十年內試算結果,它會比每年複利率定存 2% 好,你覺得如何?

2015年12月19日 星期六

ROSA 系統開發 18 ─ 六腳機器蜘蛛控制 (P&B 6 x 2 Spider)

http://4rdp.blogspot.tw/2015/12/rosa-18-p-6-x-2-spider.html

(請打開字幕觀賞)

P&B 6x2 Spider

六腳蜘蛛與四腳蜘蛛的控制程式差異不大,下面僅列出六腳蜘蛛有差異的部分。

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)


#define LAST_SERVO 12    // for 6 x 2 spider 

byte servo_revse[] = 
{ 0,  1,  1,  0,  0,  1,  1,  0,  0,  1,  0,  0,  1 };

const byte RUN_3[] PROGMEM = {              // spider dance-1
//   0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12  
_SERVO,bck,std,bck,std,mid,std,mid,std,fwd,std,fwd,std,   //0
_HOLD, 150,255,150,255,150,255,150,255,150,255,150,255,   //1
_LOOP,   3,255,255,255,255,255,255,255,255,255,255,255,   //2
_SERVO,255, 60,255,120,255, 60,255,120,255, 60,255,120,   //3
_SERVO,255,std,255,std,255,std,255,std,255,std,255,std,   //4
_NEXT,   3,255,255,255,255,255,255,255,255,255,255,255,   //5 
_LOOP,   3,255,255,255,255,255,255,255,255,255,255,255,   //6
_SERVO,255,120,255, 60,255,120,255, 60,255,120,255, 60,   //7
_SERVO,255,std,255,std,255,std,255,std,255,std,255,std,   //8
_NEXT,   7,255,255,255,255,255,255,255,255,255,255,255,   //9
_GOTO,   0,                                               //10
};

2015年12月16日 星期三

訓練數學感 82 ─ 父子的年齡是多少?

http://4rdp.blogspot.tw/2015/12/82.html

圖片來源 https://pixabay.com/
父子兩人,父說 我在你這年紀時,你才 5 歲,等你到我這年紀,我就 71 歲,請問這對父子現在年齡各多少?


2015年12月12日 星期六

ROSA 系統開發 17 ─ 四腳機器蜘蛛控制 (P&B 4 x 2 Spider)

http://4rdp.blogspot.tw/2015/12/rosa-17-p-4-x-2-spider.html

(請開啟字幕觀賞)

P&B 4x2 Spider

前一陣子在台灣 Maker 界出現一隻開源機器蜘蛛,網友雷基士 (Regis) 很全能,除了寫程式外還 3D 列印、製作電路板,而我正在開發 ROSA,這作業系統可以控制 meArm,當然也可以控制機器蜘蛛,正好網友 Pizg 設計出新型機器蜘蛛機構請我測試,因此本文以他的作品當主體展示 ROSA 紅外線遙控器操控成果,還有採用 ROSA 解決方案,不需要用到數學三角函數,很適合學生入門學習。

這款機器蜘蛛姑且稱為 P&B 4 x 2 Spider,設計理念很簡單,要讓 Maker 玩家除了蜘蛛機構本體,其它所需零組件都盡可能在現有通路買的到,以方便無電子電路基礎玩家入門學習,減少製作電路障礙,清單如下:
1. Arduino UNO x 1
2. Arduino IO 擴充板 x 1
3. SG-90 伺服馬達 x 8
4. 雙孔 USB 行動電源 (Output 5V 2.1A) x 1
5. KEYES 紅外線遙控器及模組 x 1
6. 3-pin 母對母杜邦線 x 1
7. USB 轉 TTL 傳輸線 (或是自製 USB 電源線 ) x 1 
8. 超音波模組 (可略,目前當眼睛裝飾,也可以其它物件替代) x 1

好不多說,先看 ROSA 程式怎麼寫,

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)


#define LAST_SERVO   8  // for 4 x 2 spider 
#define SERVO_TABLE  13
#define zeo 0           // zero
#define lif 45          // lift leg
#define std 90          // stand
#define hon 251         // hold on
#define hof 250         // hold off
#define fwd 60          // forward
#define mid 90          // middle
#define bck 120         // backward

byte servo_pin[] = 
{ 0,  4,  5,  6,  7,  9, 10, 11, 12, 14, 15, 16, 17 };
byte servo_revse[] = 
{ 0,  1,  0,  0,  1,  1,  1,  0,  0,  1,  0,  0,  1 };
byte servo_angle[] = 
{ 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 };
byte servo_set[] = 
{ 0, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90 };
byte servo_speed[] = 
{ 0,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5 };
byte servo_hold[] = 
{ 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 };

const byte RUN_0[] PROGMEM = {                // spider start
//   0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12  
_SPEED,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5,  5,   //0
_SERVO,fwd,lif,255,255,255,255,255,255,255,255,255,255,   //1 
_SERVO,mid,std,255,255,255,255,255,255,255,255,255,255,   //2
_SERVO,hon,hon,fwd,lif,255,255,255,255,255,255,255,255,   //3
_SERVO,255,255,mid,std,255,255,255,255,255,255,255,255,   //4
_SERVO,255,255,hon,hon,fwd,lif,255,255,255,255,255,255,   //5
_SERVO,255,255,255,255,mid,std,255,255,255,255,255,255,   //6
_SERVO,255,255,255,255,hon,hon,fwd,lif,255,255,255,255,   //7
_SERVO,255,255,255,255,255,255,mid,std,255,255,255,255,   //8
_SERVO,255,255,255,255,255,255,hon,hon,fwd,lif,255,255,   //9
_SERVO,255,255,255,255,255,255,255,255,mid,std,255,255,   //10
_SERVO,255,255,255,255,255,255,255,255,hon,hon,fwd,lif,   //11
_SERVO,255,255,255,255,255,255,255,255,255,255,mid,std,   //12
_GOTO, 212,                              // goto RUN_P    //13
};

2015年12月9日 星期三

訓練數學感 81 ─ 水果多少錢?

http://4rdp.blogspot.tw/2015/12/81.html

這題不難,小學三四年級程度題目,

2015年12月5日 星期六

ROSA 系統開發 16 ─ meArm 程序控制 (Procedure Control)

http://4rdp.blogspot.tw/2015/12/rosa-16-mearm-procedure-control.html

圖片來源 http://www.shutterstock.com
本章節主要在說明程序控制,那甚麼是程序控制?無論任何複雜機器人、機器手臂,其動作控制可以分解成許多時間串聯組合的單一小動作。以 meArm 為例,想要用它夾取物品,那需要先基座馬達轉到定位,然後手臂伸過去,夾子再夾起來,這些動作控制跟時間有關,前一個動作尚未完成前,是不能執行下一個動作的,不然就會看到機器人不聽使喚亂動一通,另外,設計機器人程序動作程式時,絕大多數系統非常複雜,有時候想增刪個小動作,可能整篇程式都修改。因此,ROSA 提供一個簡單方法,可以先將機器人動作編輯好,以代碼的型式儲存常數,未來無論是放在程式碼中或是 EEPROM,都可隨時取出依據程序控制動作。





// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)

#define LAST_SERVO   4
#define SERVO_TABLE  5
byte step_wait;

enum {
  _NONE,
  _GOTO,
  _HOLD,
  _LOOP,
  _NEXT,
  _RETURN,
  _SERVO,
  _SPEED,
  _STOP,
  _WAIT,
};

const byte RUN_N[] PROGMEM = {   // meArm stop
  _STOP,                         //1
};

const byte RUN_K[] PROGMEM = {   // base sweep
  _LOOP,   10, 255, 255, 255,   
//0 
  _SERVO,   0, 255, 255, 255,    //1 
  _SERVO, 180, 255, 255, 255,    //2 
  _NEXT,    1, 255, 255, 255,    //3 
  _STOP,                         //4 
};

const byte RUN_5[] PROGMEM = {   // speed fast
  _SPEED, 5, 5, 5, 5,            //0 
  _RETURN,                       //1 
};

const byte RUN_8[] PROGMEM = {   // speed slow
  _SPEED, 1, 1, 1, 1,            //0 
  _RETURN,                       //1 
};

const byte RUN_0[] PROGMEM = {   // meArm reset
  _SERVO, 90,  90,  90,  90,     //0 
  _HOLD,  10,  10,  10,  10,     //1 
  _RETURN,                       //2
};

const byte *ACTION[] = {
  RUN_N,           // none,
  RUN_N, // RUN_1, // 1
  RUN_N, // RUN_2, // 2
  RUN_N, // RUN_3, // 3
  RUN_N, // RUN_4, // 4
  RUN_5,           // 5
  RUN_N, // RUN_6, // 6
  RUN_N, // RUN_7, // 7
  RUN_N, // RUN_8, // 8
  RUN_N, // RUN_9, // 9,  
  RUN_0,           // 0
  RUN_N, // RUN_S, // *
  RUN_N, // RUN_P, // #
  RUN_N, // RUN_U, // up
  RUN_N, // RUN_D, // down
  RUN_N, // RUN_R, // right
  RUN_N, // RUN_L, // left
  RUN_K            // ok
};

void ACTION_SET(void){
  static byte loopn;
  byte i, j;

  switch (pgm_read_byte(ACTION[ir_code] + step_no*SERVO_TABLE)){

    case _SERVO:
      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j == 250)
         servo_hold[i] = 0;         // hold off
else if (j == 251)
         servo_hold[i] = 255;       // hold on
else if (j >=252 && j < 255)
         servo_set[i] = 90;
else if (j < 181)
         servo_set[i] = j;
      }
      break;
    case _HOLD:
      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j != 255) 
          servo_hold[i] = j;
      }
      break;
    case _SPEED:
      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j != 255) 
          servo_speed[i] = j;
      }
      break;
    case _WAIT:
      step_wait = pgm_read_byte(ACTION[ir_code] 
                  + step_no * SERVO_TABLE + 1);
      break;
    case _RETURN:
      ir_code = pre_ir;
      goto ret;
    case _NEXT:
      if (loopn != 255) {
if (loopn == 0) 
          break;
        loopn--;
      }
    case _GOTO:
      j = pgm_read_byte(ACTION[ir_code] 
          + step_no * SERVO_TABLE + 1);
      if (j >= 200){
        ir_code = j - 200;
ret:
step_no = 255;
      } else
        step_no = j - 1;
      break;
    case _LOOP:
      loopn = pgm_read_byte(ACTION[ir_code] 
              + step_no * SERVO_TABLE + 1) - 1;
      if (loopn==254) 
        loopn++;       // 0 or 255 always loop
      break;
    case _STOP:
      ir_code = 0;
  }
}

void PROCESS_SERVO(void) {
  static byte servo_no;
  byte all_ready;
  byte i;

  if (servo_no == 0 || servo_no > SERVO_TABLE) {
    servo_no = SERVO_TABLE;
  }

  if (servo_no > LAST_SERVO)
    ;
  else if (servo_set[servo_no] > servo_angle[servo_no]){
    servo_angle[servo_no] += servo_speed[servo_no];
    if (servo_set[servo_no] < servo_angle[servo_no])
      servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_set[servo_no] < servo_angle[servo_no]){
    servo_angle[servo_no] -= servo_speed[servo_no];
    if (servo_angle[servo_no] > 180)
    servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_hold[servo_no] != 0){
    if (servo_hold[servo_no] != 255)
      servo_hold[servo_no]--;
    SERVO_SET(servo_no);
  }
  servo_no--;

  if (ir_code != 0) {

    all_ready = 1;
    for (i = 1; i < LAST_SERVO+1; i++){
      if (servo_set[i] != servo_angle[i])
all_ready = 0;
    }
    if (all_ready == 1){
      if (step_wait == 0){
ACTION_SET();
step_no++;
      }
      else
step_wait--;
    }
  }
}

2015年12月2日 星期三

訓練數學感 80 ─ 摺紙的長度

http://4rdp.blogspot.tw/2015/12/80.html

一張 ABCD 20 x 30 cm 長方形紙,將其 C 點對準 AB 中點摺紙,試求 EF 線段長度。

2015年11月28日 星期六

訓練數學感 79 ─ 25 匹馬

http://4rdp.blogspot.tw/2015/11/79-25.html

有一馬場共 25 匹馬,它們的個別跑速不清楚,只知道每一匹馬都非常平穩固定,跑馬場每次最多只能 5 匹馬下場比賽,請問最少需要比賽幾場,才能分出這 25 匹馬的冠、亞、季軍是那隻馬?

2015年11月24日 星期二

ROSA 系統開發 15 ─ 回覆台大資工系洪士灝教授的提問

http://4rdp.blogspot.tw/2015/11/rosa-15.html

週日晚上,無意間看到台大洪士灝教授在 FB 貼文轉型到 Open Source 的世界來尋寶,個人蠻認同其觀點,正好個人在開發 ROSA (Robot Operation System for Arduino),因此留文請教洪教授對 ROSA 的看法,教授留言一句話「是否可以用一頁說明這個計畫的目的、優點、製作方法、所需克服的障礙 ?

我把這當作如果未來創業,不管是天使投資人 (只要講得出好點子就會投資你) 還是惡魔投資人 (問一堆問題還是不會投資,然後用你的點子賺錢) 必問的問題,其實這應該要準備一頁 Business Model Canvas 說明,日後再找時間談這個,目前對 ROSA 我以公益看待,在此不談商業模式。

經過兩天沉思後,答覆教授的問題如下:

2015年11月21日 星期六

ROSA 系統開發 14 ─ Control meArm by IR

http://4rdp.blogspot.tw/2015/11/rosa-14-control-mearm-by-ir.html

紅外線遙控能用於 meArm 嗎?答案是可以的,考量許多玩家沒有 mBot 紅外線遙控器,因此另外準備一隻遙控器以及能與 Arduino 相接的紅外線接收器,如上圖所示。

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)

const byte IR_CODE[][18] PROGMEM = {
//  X, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, *, #,up,dn,rt,lf,ok
    0,22,25,13,12,24,94, 8,28,90,82,66,74,70,21,67,68,64,
    0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
};

void (*FUNC_JUMP[])(void) = {
  FUNC_N,            // none
  FUNC_1,            // 1
  FUNC_N, // FUNC_2, // 2
  FUNC_3,            // 3
  FUNC_4,            // 4
  FUNC_N, // FUNC_5, // 5
  FUNC_6,            // 6
  FUNC_N, // FUNC_7, // 7
  FUNC_N, // FUNC_8, // 8
  FUNC_N, // FUNC_9, // 9
  FUNC_N, // FUNC_0, // 0
  FUNC_S,            // *
  FUNC_P,            // #
  FUNC_N, // FUNC_U, // up
  FUNC_N, // FUNC_D, // down
  FUNC_R,            // right
  FUNC_L,            // left
  FUNC_N, // FUNC_K  // ok
};

void PROCESS_10ms(void){
  IR_CHECK();

  if (A_IN(A4) < 200){   // right X
    if (servo_set[2] > 120) servo_set[2]--;
  }
  else if (A_IN(A4) >= 800){
    if (servo_set[2] < 160) servo_set[2]++;
    else servo_hold[2] = 255;
  }
  else
    servo_hold[2] = 0;

  if (A_IN(A0) < 200){   // left Y
    if (servo_set[4] > 60) servo_set[4]--;
  }
  else if (A_IN(A0) >= 800) {
    if (servo_set[4] < 150) servo_set[4]++;
  }
  if (A_IN(A1) < 200){   // left X
    if (servo_set[1] != 0) servo_set[1]--;
  }
  else if (A_IN(A1) >= 800){
    if (servo_set[1] < 180) servo_set[1]++;
  }
  if (A_IN(A3) < 200){   // right Y
    if (servo_set[3] > 60) servo_set[3]--;
  }
  else if (A_IN(A3) >= 800) {
    if (servo_set[3] < 150) servo_set[3]++;
  }
}

byte pre_ir;
byte ir_code;
byte step_no = 0;

void IR_CHECK() {   // for KEYES IR
  byte i;
  byte j;
  j = ir.key();
  // Serial.print("IR - ");
  // Serial.println(j);
  for (i = 17; i != 0; i--) {
    if (j == pgm_read_byte(&IR_CODE[0][i])) {
      if (pgm_read_byte(&IR_CODE[1][i])==0 || pre_ir!=j) {
        pre_ir = ir_code;
        ir_code = i;
        step_no = 0;
        (*FUNC_JUMP[i])();
break;
      }
    }
  }
  ir.loop();
}

void FUNC_N(void)  // none
{
}

void FUNC_1(void)  // left Y
{
  if (servo_set[4] < 150) servo_set[4]++;
}

void FUNC_4(void)
{
  if (servo_set[4] > 60) servo_set[4]--;
}

void FUNC_3(void)  // right Y
{
  if (servo_set[3] < 150) servo_set[3]++;
}

void FUNC_6(void)
{
  if (servo_set[3] > 60) servo_set[3]--;
}

void FUNC_S(void)  // right X
{
  if (servo_set[2] < 160) servo_set[2]++;
  else servo_hold[2] = 255;
}

void FUNC_P(void)
{
  if (servo_set[2] > 120) servo_set[2]--;
}

void FUNC_R(void)  // left X
{
  if (servo_set[1] != 0) servo_set[1]--;
}

void FUNC_L(void)
{
  if (servo_set[1] < 180) servo_set[1]++;
}

2015年11月18日 星期三

數學之美 4 ─ 解析幾何

http://4rdp.blogspot.tw/2015/11/4.html

數學式與圖形之間有密不可分的關係,這在我專科時才體悟到,後來才知道這門數學稱為解析幾何。

我的工作常常需要分析數值,利用 Curve Fitting 等技巧,求解最近似方程式,這些能力皆是建立在數學方程式與函數曲線之間關聯,非數學愛好者,很難將其聯想在一起。

2015年11月14日 星期六

ROSA 系統開發 13 ─ SERVO 多工同步控制

http://4rdp.blogspot.tw/2015/11/rosa-13-servo.html

前文談過單一伺服馬達驅動,本期續談多顆伺服馬達多工同步控制方法,

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)

byte servo_set[] = { 0, 90, 90, 90, 90};
byte servo_speed[] = { 0, 5, 1, 5, 5};
byte servo_hold[] = { 0, 0, 0, 0, 0};

void PROCESS_SERVO(void) {
  static byte servo_no;
  byte i;

  if (servo_no == 0 || servo_no > 4) {
    servo_no = 4;
  }

  if (servo_set[servo_no] > servo_angle[servo_no]){
    servo_angle[servo_no] += servo_speed[servo_no];
    if (servo_set[servo_no] < servo_angle[servo_no])
      servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_set[servo_no] < servo_angle[servo_no]){
    servo_angle[servo_no] -= servo_speed[servo_no];
    if (servo_angle[servo_no] > 180)
      servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_hold[servo_no] != 0){
    if (servo_hold[servo_no]!=255) 
      servo_hold[servo_no]--;
    SERVO_SET(servo_no);
  }
  servo_no--;
}

unsigned long  now;

void SYS_TIMER() {
  static byte  tm_10ms = 0;
  static byte  tm_100ms = 0;
  static byte  tm_1sec = 0;

while (millis() - now >= 5) {
now += 5;
tm_10ms++;
tm_100ms++;
tm_1sec++;
    PROCESS_SERVO();
}
if (tm_1sec >= 200) {
tm_1sec -= 200;
PROCESS_1sec();
}
if (tm_100ms >= 20) {
tm_100ms -= 20;
PROCESS_100ms();
}
if (tm_10ms >= 2) {
tm_10ms -= 2;
PROCESS_10ms();
}
}

void PROCESS_1sec(void){   // print analog value for debugging
  Serial.print("A0 - ");
  Serial.println(A_IN(A0));
  Serial.print("A1 - ");
  Serial.println(A_IN(A1));
  Serial.print("A2 - ");
  Serial.println(D_IN(A2));

  Serial.print("A3 - ");
  Serial.println(A_IN(A3));
  Serial.print("A4 - ");
  Serial.println(A_IN(A4));
  Serial.print("A5 - ");
  Serial.println(D_IN(A5));
}

void PROCESS_100ms(void){
}

void PROCESS_10ms(void){
  if (A_IN(A4) < 200){   // right X
    if (servo_set[2] > 120) servo_set[2]--;
  }
  else if (A_IN(A4) >= 800){
    if (servo_set[2] < 160) servo_set[2]++;
    else servo_hold[2] = 255;
  } else
    servo_hold[2] = 0;

  if (A_IN(A0) < 200){   // left Y
    if (servo_set[4] > 60) servo_set[4]--;
  }
  else if (A_IN(A0) >= 800) {
    if (servo_set[4] < 150) servo_set[4]++;
  }
  if (A_IN(A1) < 200){   // left X
    if (servo_set[1] != 0) servo_set[1]--;
  }
  else if (A_IN(A1) >= 800){
    if (servo_set[1] < 180) servo_set[1]++;
  }
  if (A_IN(A3) < 200){   // right Y
    if (servo_set[3] > 60) servo_set[3]--;
  }
  else if (A_IN(A3) >= 800) {
    if (servo_set[3] < 150) servo_set[3]++;
  }
}