FBTUG-水量設計與實驗

最後編輯:2017-05-25 建立:2017-05-23 歷史紀錄

 

WUULONG S緣起

  • FR03 實驗需要精準的水量控制, 原生 Farmbot 雖有訂定spec, 但無此function 可用.
  • JOE H

水量控制方式

  • 1. 水量控制by 毫升: (FBTUG 新增: 補齊原廠的code)
  • Command: "F02 N800\n"
  • 2. 水量控制by 毫秒: (FBTUG 新增: 補齊原廠的code)
  • Command: "F01 T800\n"

 

3. 水量控制by 毫升: (無流量計, 使用time 控制)

  • Command: "F01 N800\n"

 

  • 4. 另一種時間控制 (farmbot原廠的function)
  • Command: "F44 P9 V1 W0 T800 M0\n"
  • 註: pin9(water), V1(啟動), 在T 800ms後, W0(關閉), M0(digital mode)
  • 5. 水量控制by 手動: (farmbot原廠的function)
  • Command: "F41 P9 V1 M0\n": 開啟水閘或pump
  • Command: "F41 P9 V0 M0\n": 關閉水閘或pump

WUULONG SScope

  • JOE H此次實驗先做第一種水量控制by 毫升
  • WUULONG S1. 水壓控制: 出水量過大會影響土壤覆蓋及實驗準確度
  • JOE H2. 水量控制: 此實驗需要精準的水量控制

 

器材驗証

  1. 電動水開關:
    • 使用3D列印件 + MG995 來控制: 以下為STL 及servo 測試代碼
    • 請注意角度問題, 先調整好全關為 0度, 全開為70度.
    • 實際代碼請參考 ServoControl.cpp
    • 根據 官方命令列表 , Pin 4 & 5 可以控制Servo 的角度
    • Command 格式:
    • "F61 P4 V45\n" -- Pin4 的 servo 轉45度

 

2. 水流量傳感器 (Water Flow Sensor - 1/8" SKU)

3.微型水泵直流12V

 

使用Arduino UNO 的測試code:

  • volatile double waterFlow;
  • void setup() {
  • Serial.begin(115200); //baudrate
  • waterFlow = 0;
  • attachInterrupt(0, pulse, RISING); //DIGITAL Pin 2: Interrupt 0
  • }
  • void loop() {
  • Serial.print("waterFlow:");
  • Serial.print(waterFlow*1000);
  • Serial.println(" mL");
  • delay(500);
  • }
  • void pulse() //measure the quantity of square wave
  • {
  • waterFlow += 1.0 / 5880.0;
  • }

 

器材安裝

測試平台必須確保流量計的水流方向為 90度, 以利水流準確度.

  • 流量計 Clock pin 請接至Arduino 的Pin20(IRQ3), Arduino MEGA 2560 共有6 個外部中斷可以使用

*

 

  • 微型水泵直流12V接到Pin9
  • 電動水開關(Servo) 接到Pin4

 

接線參考圖

 

 

WUULONG S校正

  1. JOE H"F61 P4 V70\n" -- Servo 轉70度, 水量全開

2. 調整流量計的clocks 來校正水量

  • 步驟:
  • a. "F22 P300 V5200\n" -- 設定clocks 為每公升5200.
  • b. "F21 P300\n" -- 確定是否存入EEPROM
  • 原理:
  • 流量計每 1L trigger 5200 次的方波.---> 請參考各廠牌的spec (此廠牌定義default為5880)
  • 使用attachInterrupt() 的正緣觸發(RISING), 每次都發interrupt 到pulse() 來計算流量.

 

測試

"F02 N200\n" -- 啟動馬達 , 到達 200ml 水量後停止.

 

測試連結

https://www.facebook.com/groups/FarmBotTUG/permalink/793007324215327/

 

代碼連結

https://github.com/FBTUG/DevZone/tree/master/mini-farmbot/farmbot-arduino-FBTUG

 

結論

設定 5200 clocks, 200ml 的水量, 可以到5cc 左右的誤差.

大約落在195~200cc, 實際farmbot 測試,須根據實際狀況再調整.

 

--------------------------------------------------------------------------------------------------------------------------------

2017/05/26

 

水量實驗by時間

  • 此次實驗改用時間控制, 單位雖然為毫升,但實際是時間控制.

WUULONG SScope

  • JOE H當流量計的準確度不足 ,或手邊沒有流量計時, 這是另一個好方法.

新增功能

更改 F01 handle, 新增N 為水量, 讓操作更直覺.

新增EEPROM P301, 每注入100ml所須的時間 (ms)

 

測試

"F22 P301 V4635\n" --> 設定校正用單位流量,每100ml所須的時間 (ms)

"F21 P301\n" --> 確認已被存入

"F01 N100\n"--> 供水100ml 並關水

"F01 N50\n"--> 供水50ml 並關水

 

結論

沒想到竟然沒有誤差~ 這顆水泵,每100ml 要4635ms, 順利達到 100g,150g的設定!

 

測試連結

https://www.facebook.com/groups/FarmBotTUG/permalink/794290307420362/

 

代碼連結

暫時commit 於

https://github.com/joehou45/Farmbot_FBTUG