belt_conveyor_ex01 のバックアップ(No.1)

バックアップ一覧
差分を表示
現在との差分を表示
ソースを表示
belt_conveyor_ex01 へ行く。
- 1 (2023-11-05 (日) 00:56:34)

Real2Virtual202111

slide_table_ex04.c †

Stepping motor controller, Arduino, with ToF sensor, LCD,
- references
  - http://www.wsnak.com/wsnakblog/?p=2170

 
-
|
|
|
|
|
|
|
|
|
!
 
-
!
 
-
|
!
-
!
 
 
-
|
|
|
|
!
 
 
 
 
 
 
 
 
 
 
 
 
-
|
-
-
|
!
|
|
|
|
|
-
|
|
!
|
!
-
-
|
|
|
-
!
-
|
!
|
|
|
|
|
-
!
|
|
-
!
|
|
|
|
|
-
!
-
!
|
|
|
|
|
|
-
!
|
|
|
|
|
|
|
|
|
-
|
!
|
|
|
-
|
!
!
 
-
|
-
|
!
|
!
-
-
|
|
!
|
!
 
-
|
|
!
 
-
|
|
-
-
|
|
!
|
|
-
!
-
!
|
-
-
|
|
|
|
-
!
!
-
!
-
!
|
!
 
-
|
|
|
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|

#include <M5Stack.h>
/*
  M5Stack Basic V2.6端子仕様
  G3／G1  ：UART0通信用（RX／TX）　※標準のシリアル通信未使用なら入力／出力使用可、書き込み時は未接続にする
  G16／G17：UART2通信用（RX／TX）　※シリアル通信2未使用ならG16は入力／出力、G17は出力のみ使用可
  G2／G5  ：入力／出力／G2のみアナログ入力可
  G25／G26：G25はスピーカー専用、G26は入力／出力／アナログ入力／アナログ出力
  G35／G36：アナログ入力（入力としても使用できるがプルアップ不可）
  G21／G22：I2C通信用 Groveコネクタと共通(SDA／SCL）　※入出力使用可だが電源ICと繋がっているため非推奨
  G23／G19／G18：SPI通信用（MOSI／MISO／SCK）　※G19のみ入出力使用可だがTF(SD)Cardと繋がっているため非推奨
  G39／G38／G37：本体ボタン（左からボタンA,B,C、内部で接続）
*/
 
// 端子割り付け
#define HB1 2   // H bridge 1, 出力
#define HB2 5   // H bridge 2, 出力
//#define ADC0 35 // アナログ入力端子
//#define DAC0 26 // アナログ出力端子
 
// constant
#define ADDRESS 0x52
#define MESSAGE_MAX 5
 
// 変数宣言
//float ad_val;         // アナログ入力値格納用
//float da_val;         // アナログ出力値指定用
//float v_in = 0;       // アナログ入力電圧換算値
//float v_out = 0;      // アナログ出力電圧換算用
int cnt = 0;          // 表示回数カウント用
String data = "";     // シリアル通信の受信データ格納用
String goState ="STOP";
String lines[MESSAGE_MAX];
String inputString;
 
uint16_t distance;     //tof
uint16_t distance_tmp; //tof
uint8_t data_cnt;      //tof
 
uint8_t message_line;   //
 
 
// 関数 ************************************************************************
// シリアルモニタ出力処理
void return_result(String data) {
    for(int i=0;i<MESSAGE_MAX-1;i++){
      lines[i]=lines[i+1];
    }
    lines[MESSAGE_MAX-1]=data;
    M5.Lcd.fillRect(205, 52, 115, 18, WHITE);
    M5.Lcd.setTextFont(2);                // フォント
    M5.Lcd.setCursor(210, 53); M5.Lcd.setTextColor(BLACK);  // 座標指定(x, y) // 文字色、背景
    M5.Lcd.fillRect(0, 52, 320, MESSAGE_MAX*18, WHITE);   // 塗りつぶし四角
    for(int i=0;i<MESSAGE_MAX;i++){
        M5.Lcd.setCursor(0,50+i*18);
        M5.Lcd.print(lines[i]);                   // 受信データ液晶表示
    }
    Serial.println(data);                 // 受信データ標準シリアル出力
}
// 初期設定 ************************************************************************
void setup(){
  M5.begin();                 // 本体初期化(LCD, SD, Serial, I2C)
  M5.Power.begin();           // 電源初期化
  Serial.begin(115200);         // 標準のシリアル通信初期化（初期値はG3（RX）,G1（TX））
  //Serial2.begin(9600);        // シリアル通信2初期化 (初期値は G16（RX）, G17（TX）)
 
  // 入出力ピン設定
  // 出力設定
  pinMode(HB1, OUTPUT);      // to H bridge control input
  pinMode(HB2, OUTPUT);      // to H bridge control output
  digitalWrite(HB1, LOW);    //
  digitalWrite(HB2, LOW);    // set stop
  goState="STOP";
  
  //LCD表示
  M5.Lcd.fillScreen(BLACK); // 背景色
  M5.Lcd.setTextSize(1);    // 文字サイズ（整数倍率）
  M5.Lcd.setTextFont(2);    // フォント 1(8px), 2(16px), 4(26px)
                                    // 6(36px数字-.:apm), 7(7セグ-.:), 8(75px数字-.:
  M5.Lcd.drawFastHLine(0, 0, 320, WHITE);           // 指定座標から平行線
  M5.Lcd.fillRect(0, 1, 320, 13, (uint16_t)0x098a); // タイトルエリア背景
  M5.Lcd.drawFastHLine(0, 20, 320, WHITE);          // 指定座標から平行線
  M5.Lcd.setTextColor(WHITE);                       // 文字色
  M5.Lcd.drawCentreString("Belt Conver and ToF", 160, 4, 2);  //　指定座標を中心に文字表示（x, y, font）
 
//  M5.Lcd.drawFastVLine(0, 29, 320, WHITE);   // 指定座標から垂直線（x, y, l, 色）
  M5.Lcd.drawFastHLine(0, 149, 320, WHITE);    // 指定座標から平行線（x, y, l, 色）
//  M5.Lcd.drawFastHLine(0, 132, 320, WHITE);
  M5.Lcd.drawFastHLine(0, 192, 320, WHITE);
  M5.Lcd.drawCircle(10, 161, 9, WHITE);        // 指定座標で半径を指定して円描画（x, y, r, 色）
 
  M5.Lcd.setTextFont(2);                      // フォント
  M5.Lcd.setCursor(0, 28);    M5.Lcd.print("SERIAL MONITOR");   // シリアルモニタ項目表示
  M5.Lcd.fillRect(0, 52, 320, MESSAGE_MAX*18, WHITE);   // 塗りつぶし四角
 
  //本体ボタン項目表示
  M5.Lcd.drawRect(37, 216, 66, 23, WHITE);    // 本体ボタン表示枠用四角
  M5.Lcd.drawRect(127, 216, 66, 23, WHITE);
  M5.Lcd.drawRect(217, 216, 66, 23, WHITE);
 
  M5.Lcd.setTextFont(2);                                // フォント
  M5.Lcd.setCursor(55, 221);    M5.Lcd.print("GO");   // H Bridge Control
  M5.Lcd.setCursor(135, 221);   M5.Lcd.print("BACK"); // H Bridge Control
  M5.Lcd.setCursor(225, 221);   M5.Lcd.print("STOP"); // ON/OFFボタン
 
  message_line=0;
  for(int i=0;i<MESSAGE_MAX;i++){
    lines[i]="";
  }
 
  Serial.println("MTOF171000C0_I2C");
  Wire.begin();
  //M5.Lcd.println("MTOF171000C0_I2C");
  //pinMode(18, OUTPUT);
  delay(1000);
}
 
String rb(String x){
  String rtn=x;
  while(rtn.startsWith(" ")){
    rtn=rtn.substring(1);
  }
  return rtn;
}
boolean rkey(String key, String x, String *rest){
   if(x.startsWith(key)){
     rest[0]=x.substring(key.length());
     return true;
   }
   return false;
}
 
boolean isDec(char c){
  if('0'<=c && c<='9') return true;
  return false;
}
 
boolean rpint(String x, int n[], String *rest){
   int nx=0;
   int cx=0;
   //return_result("rping "+x);
   if(!isDec(x.charAt(0))){
      rest[0]=x;
      return false;
   }
   cx=x.charAt(0)-'0';
   nx=cx;
   //return_result("rpint-1");
   n[0]=nx;
   //return_result("rpint-2");
   if(x.length()<=0) return true;
   x=x.substring(1);
   //return_result("rpint-3 x="+x);
   while(isDec(x.charAt(0))){
      cx=x.charAt(0)-'0';
      nx=nx*10+cx;
      if(x.length()<=0) break;
      x=x.substring(1);
      //return_result("rpint-4 x="+x+"nx="+String(nx));
      if(x.length()<=0) break;
   }
   //return_result("rpint-5");
   rest[0]=x;
   //return_result("rpint-6");
   n[0]=nx;
   return true;
}
 
boolean rStrConst(String in, String strc[], String rest[]){
  String r="";
   if(in.indexOf("\"")!=0) return  false;
   in=in.substring(1);
   while(in.indexOf("\"")!=0){
        if(in.length()<=0) return false;
        if(in.charAt(0)=='\\'){
            in=in.substring(1);
            if(in.length()<=0) return false;
            char x=in.charAt(0);
            if(x=='n'){
               r=r+"\n";
            }
            else
            if(x=='"'){
               r=r+"\"";
            }
            else
            {
              r=r+in.charAt(0);
            }
        }
        else{
          char x=in.charAt(0);
          r=r+in.charAt(0);
        }
        in=in.substring(1);
   }
   strc[0]=r;
   rest[0]=in;
   return true;
}
 
 
boolean rHex(String in, long hex[], String rest[]){
  String r;
  long lx;
  lx=0;
   if(in.indexOf("0x")!=0) return false;
    // Serial.println("rHex "+in+" ");
    in=in.substring(2);
   if(in.length()<=0) return false;
   char x=in.charAt(0);
   while(isHexadecimalDigit(x)){
      if(isDigit(x)){
        lx=lx*16+x-'0';
      }
      else
      if(('a'<=x) && x<='f'){
         lx=lx*16+x-'a'+10;
      }
      else
      if('A'<=x && x<='F'){
         lx=lx*16+x-'A'+10;
      }
       in=in.substring(1);
       if(in.length()<=0) break;
       x=in.charAt(0);
   }
   hex[0]=lx;
   rest[0]=in;
   //Serial.println(hex[0],HEX);
   return true;
}
 
boolean rInt(String in, long ri[], String rest[]){
  //Serial.print("rInt "+in+" ");
  String r;
  int sign;
  long lx;
  lx=0;
  sign=1;
  if(in.indexOf("-")==0){
     sign=-1;
     in=in.substring(1);
  }
  if(in.length()<=0) return false;
  char x=in.charAt(0);
  if(!isDigit(x)) return false;
  while(isDigit(x)){
     lx=lx*10+x-'0';
     in=in.substring(1);
     if(in.length()<=0) break;
     x=in.charAt(0);
  }
  ri[0]=sign*lx;
  //Serial.println(ri[0]);
  rest[0]=in;
   return true;
}
 
int thresh=0; 
/*
command:
get distance. //get current tof results (length).
get thresh. //get current threshold.
set thresh <length(mm)>.  //if tof results less than <length>, back. else stop. if 0, disable.
go.
back. 
stop.
*/
void parse_line(String line){
  String l=rb(line);
  String rest[1];
  int nx[1];
  Serial.println("l="+l);
  if(rkey("get ",l,rest)){
     l=rb(rest[0]);
     //DebugSerial.println("l="+l);
     if(rkey("distance",l,rest)){
           String sx=String(distance);
           //DebugSerial.println("sx="+sx);
           return_result(sx);
     }
     else
     if(rkey("thresh",l,rest)){
           String sx=String(thresh);
           //DebugSerial.println("sx="+sx);
           return_result(sx);
     }
 
  }
  else
  if(rkey("set ",l,rest)){
      l=rb(rest[0]);
      int n;
      //DebugSerial.println("l="+l);
      if(rkey("thresh ",l,rest)){
         l=rb(rest[0]);
         if(rpint(l,nx,rest)){
             n=nx[0];
             thresh=n;
             return_result("set thresh "+String(n));
             //DebugSerial.println("n=");
             //DebugSerial.println(n);
             
         }
      }
  }
  else
  if(rkey("go",l,rest)){
    goState="GO";
    thresh=0;
  }
  else
  if(rkey("back",l,rest)){
     goState="BACK";
     thresh=0;
  }
  else
  if(rkey("stop",l,rest)){
    goState="STOP";
    thresh=0;
  }   
}
 
// メイン処理 ************************************************************************
void loop() {
  M5.update();  // ボタン状態更新
 
  // 本体ボタン、外部ボタン入力処理
  // ボタンA または 外部スイッチ赤が押されている なら
  if (M5.BtnA.isPressed() ) {
    Serial.println("GO");    // 標準のシリアル出力
    //Serial2.println("V_UP!");   // シリアル2出力
    //da_val++;                   // アナログ出力 +1
    thresh=0;
    goState="GO";
  }
  // ボタンB または 外部スイッチ青が押されてる なら
  if (M5.BtnB.isPressed() ) {
    Serial.println("BACK");  // 標準のシリアル出力
    //Serial2.println("V_DOWN!"); // シリアル2出力
    //da_val--;                   // アナログ出力 -1
    thresh=0;
    goState="BACK";
  }
  // ボタンC または 外部スイッチ青が押されてる なら
  if (M5.BtnC.isPressed() ) {
    Serial.println("STOP");  // 標準のシリアル出力
    //Serial2.println("V_DOWN!"); // シリアル2出力
    //da_val--;                   // アナログ出力 -1
    thresh=0;
    goState="STOP";
  }
 
  // LCD表示処理
  // ON／OFFボタン出力状態表示
  M5.Lcd.setTextFont(4);                  // フォント
  M5.Lcd.setCursor(26, 153);               // 座標指定(x, y)
  if (goState.equals("GO")) {
    digitalWrite(HB1, LOW);
    digitalWrite(HB2, HIGH);
    M5.Lcd.setTextColor(CYAN, BLACK);     // (文字色, 背景)
    M5.Lcd.printf("GO        ");        //　出力端子の状態表示
    M5.Lcd.fillCircle(10, 164, 8, RED);  //　指定座標に半径を指定して塗り潰し円描画（x, y, r, 色）
  } 
  else 
  if (goState.equals("BACK")){
    digitalWrite(HB1, HIGH);
    digitalWrite(HB2, LOW); 
    M5.Lcd.setTextColor(ORANGE, BLACK);   // (文字色, 背景)
    M5.Lcd.printf("BACK     ");      //　出力端子の状態表示
    M5.Lcd.fillCircle(10, 164, 8, ORANGE); //　指定座標に半径を指定して塗り潰し円描画（x, y, r, 色）
  }
  else
  if (goState.equals("STOP")){
    digitalWrite(HB1, LOW);
    digitalWrite(HB2, LOW);
    M5.Lcd.setTextColor(MAGENTA, BLACK);   // (文字色, 背景)
    M5.Lcd.printf("STOP     ");      //　出力端子の状態表示
    M5.Lcd.fillCircle(10, 164, 8, GREEN); //　指定座標に半径を指定して塗り潰し円描画（x, y, r, 色）  
  }
 
  // バッテリー残量表示（表示更新10回に1回）
  if (cnt == 10) {                        // サンプリング
    M5.Lcd.setTextFont(1);                // フォント
    M5.Lcd.setCursor(295, 228); M5.Lcd.setTextColor(DARKGREY, BLACK); // 座標指定(x, y) // 文字色、背景
    M5.Lcd.printf("%d%%", M5.Power.getBatteryLevel());  // バッテリー残量表示
    cnt = 0;                              // サンプリング回数0リセット
  }
 
  //digitalWrite(18, LOW);
  delay(5);
//  Serial.print("distance = ");
 
  Wire.beginTransmission(ADDRESS);
  Wire.write(0xD3);
  Wire.endTransmission(false);
  Wire.requestFrom(ADDRESS, 2);
  data_cnt = 0;
  int dx = 0;
  distance_tmp = 0;
  while(Wire.available())
  {
    distance_tmp = Wire.read();
    dx = (dx << (data_cnt*8)) | distance_tmp;
    data_cnt++;
  }
  distance=dx;
 
//  String lx=""+String(distance)+" mm";
//  return_result(lx);
  if(thresh>0){
    if(distance>thresh){
      goState="GO";
    }
    else{
      goState="STOP";
    }
  }
  delay(45);
 
  while (Serial.available() > 0) {
    char inChar = (char)Serial.read();
    // Serial.print(inChar);
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      //putLine(inputString);
      parse_line(inputString);
      inputString="";
      //digitalWrite(13,LED_IS_ON);
      //LED_IS_ON=1-LED_IS_ON;
    }
  }
 
  cnt++;      // 表示回数カウント +1
  delay(100); // 遅延時間
 
}

Counter: 42, today: 1, yesterday: 0