slide_table_ex04.c のバックアップ(No.2)

バックアップ一覧
差分を表示
現在との差分を表示
ソースを表示
slide_table_ex04.c へ行く。
- 1 (2023-08-08 (火) 00:01:57)
- 2 (2023-08-08 (火) 00:55:28)
- 3 (2023-08-10 (木) 00:48:17)

Real2Virtual202111

slide_table_ex04.c †

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

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

#include <Wire.h>
 
#include <wI2cLcdACM1602.h>   // LCD
 
 
wI2cLcdACM1602 lcd;           //
 
char StrBuf[17];
 
 
uint8_t read_line[128];
int read_line_len;
String line_1st;
 
uint8_t prev_line[128];
int prev_line_len;
String line_2nd;
String inputString;
 
void putLine(String x){
  line_1st=line_2nd;
  line_2nd=x;
  char lcdl[16];
  char ch;
  int i;
  for(i = 0; i < 16; i++) {lcdl[i]=' ';}
  for(i=0; i<16;i++){lcdl[i]=line_1st.charAt(i);}
  lcd.setCursor(0,0);
  for(i=0;i<16;i++) {lcd.print(lcdl[i]);}
  
  for(i = 0; i < 16; i++) {lcdl[i]=' ';}
  for(i=0; i<16;i++){lcdl[i]=line_2nd.charAt(i);}
  lcd.setCursor(0,1);
  for(i=0;i<16;i++) {lcd.print(lcdl[i]);}
}
 
 
int LED_IS_ON;
 
/*
Slide Table ex01
*/
 
/*
  Control a bi-polar stepper motor using the SparkFun ProDriver TC78H670FTG
  By: Pete Lewis
  SparkFun Electronics
  Date: July 2nd, 2020
  License: MIT. See license file for more information but you can
  basically do whatever you want with this code.
 
  This example does a custom setup (1/2 microstep resolution) and turns the motor back and forth.
  
  Feel like supporting open source hardware?
  Buy a board from SparkFun! https://www.sparkfun.com/products/16836
 
  Hardware Connections:
 
  ARDUINO --> PRODRIVER
  D8 --> STBY
  D7 --> EN
  D6 --> MODE0
  D5 --> MODE1
  D4 --> MODE2
  D3 --> MODE3
  D2 --> ERR
 
*/
 
//#include <Wire.h>
 
#define ADDRESS 0x52
 
uint16_t distance;
uint16_t distance_tmp;
uint8_t data_cnt;
 
#include "SparkFun_ProDriver_TC78H670FTG_Arduino_Library.h" //Click here to get the library: http://librarymanager/All#SparkFun_ProDriver
PRODRIVER myProDriver; //Create instance of this object
 
uint32_t current_pulse;
double position; //mm.
uint32_t zero_pulse;
int direction;
 
void setup() {
  Serial.begin(115200);
  Serial.println("slide table ex03, SparkFun ProDriver TC78H670FTG, distance...mtof171010c0");
 
  pinMode(9, INPUT_PULLUP);
  pinMode(10, INPUT_PULLUP);
 
  //***** Configure the ProDriver's Settings *****//
  // Note, we must change settings BEFORE calling the .begin() function.
  // For this example, we will try 1/2 step resolution.
 // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_2;
 
  // The following lines of code are other options you can try out.
  // Comment-out the above settings declaration, and uncomment your desired setting below.
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_FULL;
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_2; // 1/2 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_4; // 1/4 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_8; // 1/8 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_16; // 1/16 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_32; // 1/32 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_64; // 1/64 step
  // myProDriver.settings.stepResolutionMode = PRODRIVER_STEP_RESOLUTION_FIXED_1_128; // 1/128 step
  direction=0;
  current_pulse=0;
  position=0.0;
  myProDriver.begin(); // adjust custom settings before calling this
 
  Wire.begin();
  pinMode(13, OUTPUT);
  
  lcd.begin();        // LCD初期化
 
  lcd.clear();        // 全クリア
  lcd.noBlink();      // カーソル点滅なし
  lcd.noCursor();     // カーソル表示なし
 
 
  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;
   if(!isDec(x.charAt(0))){
      rest[0]=x;
      return false;
   }
   cx=x.charAt(0)-'0';
   nx=cx;
   n[0]=nx;
   if(x.length()<=0) return true;
   x=x.substring(1);
   while(isDec(x.charAt(0))){
      cx=x.charAt(0)-'0';
      nx=nx*10+cx;
      if(x.length()<=0) break;
      x=x.substring(1);
   }
   rest[0]=x;
   n[0]=nx;
}
 
void return_result(String x){
  Serial.println(x);
}
 
void setPulse(uint16_t x){
  current_pulse =x;
}
 
void setPosition(double x){
  position =x;
}
 
int16_t pos2pul(double x){
  uint32_t rtn=(int)(100.0*x);
  return rtn;
}
 
void action(int32_t x){
  Serial.println("action("+String(x)+")");
  if(x>0){
    myProDriver.step(x, direction); // turn 200 steps, CW direction
    current_pulse=current_pulse+x;
  }
  else{
    myProDriver.step(-x, 1-direction); // turn 200 steps, CW direction
    current_pulse=current_pulse+x;
  }
}
 
void moveTo(double x){
  Serial.println("moveto("+String(x)+")");
  double relative=x-position;
  int32_t r_pulse=pos2pul(relative);
  action(r_pulse);
  position=x;
}
 
/*
command:
get current.
set current <position>.
moveto <position>.
reverse 
reset
*/
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("current",l,rest)){
           String sx="current="+String(position);
           //DebugSerial.println("sx="+sx);
           return_result(sx);
     }
      if(rkey("distance",l,rest)){
           String sx="distance="+String(distance)+" mm.";
           //DebugSerial.println("sx="+sx);
           return_result(sx);
     }
     Serial.println("....");
  }
  else
  if(rkey("set ",l,rest)){
      l=rb(rest[0]);
      int n;
      //DebugSerial.println("l="+l);
      if(rkey("current ",l,rest)){
         l=rb(rest[0]);
         if(rpint(l,nx,rest)){
             n=nx[0];
             //DebugSerial.println("n=");
             //DebugSerial.println(n);
             position=(double)n;
             if(rkey(".",l,rest)){
               double sub1=0.0;
               if(rpint(l,nx,rest)){
                   n=nx[0];
                   for(int i=1;i<=10;i++){
                     sub1=(double)n/(pow(10.0,i));
                     if(sub1<1.0) break;
                   }
                   position=position+sub1;
               }
             }
             String sx="set current position="+String(position);
             return_result(sx);
         }
      }
  }
  else
  if(rkey("moveTo ",l,rest)){
    l=rb(rest[0]);
    if(rpint(l,nx,rest)){
      int n=nx[0];
      //DebugSerial.println("n=");
      //DebugSerial.println(n);
      double px=(double)n;
      if(rkey(".",l,rest)){
        double sub1=0.0;
        if(rpint(l,nx,rest)){
          n=nx[0];
          for(int i=1;i<=10;i++){
            sub1=(double)n/(pow(10.0,i));
            if(sub1<1.0) break;
          }
          px=px+sub1;
        }
      }
      String sx="move current position to="+String(position);
      moveTo(px);
      return_result(sx);
    }
  }
  else
  if(rkey("reverse",l,rest)){
     direction=1-direction;
  }
  else
  if(rkey("reset",l,rest)){
     direction=0;
     current_pulse=0;
     position=0.0;
  }   
}
 
 
unsigned long last_tx_time = 0;
 
/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void xserialEvent() {
  while (Serial.available()) {
    // get the new byte:
    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="";
    }
  }
}
 
void loop() {
  digitalWrite(13, LOW);
  delay(5);
  //Serial.print("distance = ");
 
  Wire.beginTransmission(ADDRESS);
  Wire.write(0xD3);
  Wire.endTransmission(false);
  Wire.requestFrom(ADDRESS, 2);
  data_cnt = 0;
  distance = 0;
  distance_tmp = 0;
  while(Wire.available())
  {
    distance_tmp = Wire.read();
    distance = (distance << (data_cnt*8)) | distance_tmp;
    data_cnt++;
  }
 
//  Serial.print(distance);
//  Serial.println(" mm");
  digitalWrite(13, HIGH);
  delay(45);
 
  if(!digitalRead(9)){
    myProDriver.step(200, 0); // turn 200 steps, CW direction
    delay(20);
  }
  if(!digitalRead(10)){
    myProDriver.step(200, 1); // turn 200 steps, CCW direction
    delay(20);
  }
  xserialEvent();
}

Counter: 101, today: 1, yesterday: 0