Test

Dependencies:   X_NUCLEO_IHM01A1

Files at this revision

API Documentation at this revision

Comitter:
jackcassa1967
Date:
Sun Dec 29 18:36:23 2019 +0000
Parent:
2:35f13b7f3659
Commit message:
Start

Changed in this revision

Tokenizer.cpp Show annotated file Show diff for this revision Revisions of this file
Tokenizer.h Show annotated file Show diff for this revision Revisions of this file
X_NUCLEO_IHM01A1.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 35f13b7f3659 -r 24ab1c8fefef Tokenizer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Tokenizer.cpp	Sun Dec 29 18:36:23 2019 +0000
@@ -0,0 +1,125 @@
+///////////////////////////////////////////////////////////////////////////////
+// Tokenizer.cpp
+// =============
+// General purpose string tokenizer (C++ string version)
+//
+// The default delimiters are space(" "), tab(\t, \v), newline(\n),
+// carriage return(\r), and form feed(\f).
+// If you want to use different delimiters, then use setDelimiter() to override
+// the delimiters. Note that the delimiter string can hold multiple characters.
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-05-25
+// UPDATED: 2011-03-08
+///////////////////////////////////////////////////////////////////////////////
+
+#include "Tokenizer.h"
+
+
+///////////////////////////////////////////////////////////////////////////////
+// constructor
+///////////////////////////////////////////////////////////////////////////////
+Tokenizer::Tokenizer() : buffer(""), token(""), delimiter(DEFAULT_DELIMITER)
+{
+    currPos = buffer.begin();
+}
+
+Tokenizer::Tokenizer(const std::string& str, const std::string& delimiter) : buffer(str), token(""), delimiter(delimiter)
+{
+    currPos = buffer.begin();
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// destructor
+///////////////////////////////////////////////////////////////////////////////
+Tokenizer::~Tokenizer()
+{
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// reset string buffer, delimiter and the currsor position
+///////////////////////////////////////////////////////////////////////////////
+void Tokenizer::set(const std::string& str, const std::string& delimiter)
+{
+    this->buffer = str;
+    this->delimiter = delimiter;
+    this->currPos = buffer.begin();
+}
+
+void Tokenizer::setString(const std::string& str)
+{
+    this->buffer = str;
+    this->currPos = buffer.begin();
+}
+
+void Tokenizer::setDelimiter(const std::string& delimiter)
+{
+    this->delimiter = delimiter;
+    this->currPos = buffer.begin();
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return the next token
+// If cannot find a token anymore, return "".
+///////////////////////////////////////////////////////////////////////////////
+std::string Tokenizer::next()
+{
+    if(buffer.size() <= 0) return "";           // skip if buffer is empty
+
+    token.clear();                              // reset token string
+
+    this->skipDelimiter();                      // skip leading delimiters
+
+    // append each char to token string until it meets delimiter
+    while(currPos != buffer.end() && !isDelimiter(*currPos))
+    {
+        token += *currPos;
+        ++currPos;
+    }
+    return token;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// skip ang leading delimiters
+///////////////////////////////////////////////////////////////////////////////
+void Tokenizer::skipDelimiter()
+{
+    while(currPos != buffer.end() && isDelimiter(*currPos))
+        ++currPos;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return true if the current character is delimiter
+///////////////////////////////////////////////////////////////////////////////
+bool Tokenizer::isDelimiter(char c)
+{
+    return (delimiter.find(c) != std::string::npos);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// split the input string into multiple tokens
+// This function scans tokens from the current cursor position.
+///////////////////////////////////////////////////////////////////////////////
+std::vector<std::string> Tokenizer::split()
+{
+    std::vector<std::string> tokens;
+    std::string token;
+    while((token = this->next()) != "")
+    {
+        tokens.push_back(token);
+    }
+
+    return tokens;
+}
diff -r 35f13b7f3659 -r 24ab1c8fefef Tokenizer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Tokenizer.h	Sun Dec 29 18:36:23 2019 +0000
@@ -0,0 +1,56 @@
+///////////////////////////////////////////////////////////////////////////////
+// Tokenizer.h
+// ===========
+// General purpose string tokenizer (C++ string version)
+//
+// The default delimiters are space(" "), tab(\t, \v), newline(\n),
+// carriage return(\r), and form feed(\f).
+// If you want to use different delimiters, then use setDelimiter() to override
+// the delimiters. Note that the delimiter string can hold multiple characters.
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-05-25
+// UPDATED: 2011-03-08
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef TOKENIZER_H
+#define TOKENIZER_H
+
+#include <string>
+#include <vector>
+
+// default delimiter string (space, tab, newline, carriage return, form feed)
+const std::string DEFAULT_DELIMITER = " \t\v\n\r\f";
+
+class Tokenizer
+{
+public:
+    // ctor/dtor
+    Tokenizer();
+    Tokenizer(const std::string& str, const std::string& delimiter=DEFAULT_DELIMITER);
+    ~Tokenizer();
+
+    // set string and delimiter
+    void set(const std::string& str, const std::string& delimiter=DEFAULT_DELIMITER);
+    void setString(const std::string& str);             // set source string only
+    void setDelimiter(const std::string& delimiter);    // set delimiter string only
+
+    std::string next();                                 // return the next token, return "" if it ends
+
+    std::vector<std::string> split();                   // return array of tokens from current cursor
+
+protected:
+
+
+private:
+    void skipDelimiter();                               // ignore leading delimiters
+    bool isDelimiter(char c);                           // check if the current char is delimiter
+
+    std::string buffer;                                 // input string
+    std::string token;                                  // output string
+    std::string delimiter;                              // delimiter string
+    std::string::const_iterator currPos;                // string iterator pointing the current position
+
+};
+
+#endif // TOKENIZER_H
diff -r 35f13b7f3659 -r 24ab1c8fefef X_NUCLEO_IHM01A1.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IHM01A1.lib	Sun Dec 29 18:36:23 2019 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM01A1/#974ca699c792
diff -r 35f13b7f3659 -r 24ab1c8fefef main.cpp
--- a/main.cpp	Thu Nov 23 13:09:25 2017 +0000
+++ b/main.cpp	Sun Dec 29 18:36:23 2019 +0000
@@ -1,31 +1,608 @@
+/**
+******************************************************************************
+* @file    main.cpp
+* @author  Davide Aliprandi, STMicroelectronics
+* @version V1.0.0
+* @date    October 14th, 2015
+* @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
+*          Motor Control Expansion Board: control of 2 motors.
+******************************************************************************
+* @attention
+*
+* <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+*
+* Redistribution and use in source and binary forms, with or without modification,
+* are permitted provided that the following conditions are met:
+*   1. Redistributions of source code must retain the above copyright notice,
+*      this list of conditions and the following disclaimer.
+*   2. Redistributions in binary form must reproduce the above copyright notice,
+*      this list of conditions and the following disclaimer in the documentation
+*      and/or other materials provided with the distribution.
+*   3. Neither the name of STMicroelectronics nor the names of its contributors
+*      may be used to endorse or promote products derived from this software
+*      without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+******************************************************************************
+*/
+
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
 #include "mbed.h"
 
-void print_char(char c = '*')
-{
-    printf("%c", c);
-    fflush(stdout);
+/* Helper header files. */
+#include "DevSPI.h"
+
+/* Component specific header files. */
+#include "L6474.h"
+#include "L6474_def.h"
+
+//#include "steppermotor.h"
+
+
+/* Definitions ---------------------------------------------------------------*/
+
+/* Number of steps. */
+#define STEPS 3200
+
+/* Delay in milliseconds. */
+#define DELAY_1 2000
+#define DELAY_2 6000
+#define DELAY_3 8000
+
+/* Speed in pps (Pulses Per Second).
+In Full Step mode: 1 pps = 1 step/s).
+In 1/N Step Mode:  N pps = 1 step/s). */
+#define SPEED_1 2400
+#define SPEED_2 1200
+
+
+#include "Tokenizer.h"
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Motor Control Component. */
+L6474 *motor1;
+L6474 *motor2;
+L6474 *motor3;
+
+int step_mode=0;
+
+
+#define M3
+#define M2
+
+
+long serial_count=0;
+
+InterruptIn InterA4(A4);
+InterruptIn InterA3(A3);
+
+DigitalIn PinA4 (A4);
+DigitalIn PinA3 (A3);
+
+DigitalOut POWER(A5);
+
+DigitalOut myled(LED1);
+
+
+
+
+Serial Console(USBTX, USBRX, 115200);
+
+
+
+
+void alarm_pinA4_on(){
+  printf("ON A4\n");
+  myled=1;
+}
+
+void alarm_pinA4_off(){
+  printf("OFF A4\n");
+  myled=0;
+}
+
+void alarm_pinA3_on(){
+  printf("ON A3\n");
+}
+
+void alarm_pinA3_off(){
+  printf("OFF A3\n");
+}
+
+
+
+
+PlatformMutex stdio_mutex;
+
+void sendString(char * line){
+  
+   stdio_mutex.lock();
+  printf("%s\n",line);
+   stdio_mutex.unlock();
+  
+}
+
+
+void SendStatus(){
+  unsigned int statusRegister = motor2->get_status();
+  char buffer[100] ;
+  sprintf(buffer,"%s %d\n","STATUS",statusRegister);
+  sendString(buffer);
 }
 
-Thread thread;
+
+
+
 
-DigitalOut led1(LED1);
+int RangeX=0;
+char bufferCommand[100] ;
+
+char * test;
 
-void print_thread()
-{
-    while (true) {
-        wait(1);
-        print_char();
+void processLine(string line){
+  
+  test=(char *)line.c_str();
+  
+  Tokenizer parser(line," ,");
+  
+  //sring sId=parser.next();
+  
+  string command = parser.next();
+  
+  if (command=="POWER"){
+    
+    string SW = parser.next();    
+    int iSW=atoi(SW.c_str());    
+    if (iSW==0){
+      POWER=1;
+  
+    }
+    else{
+      POWER=0;
+      wait_ms(5000);
+      motor2->enable();
+      motor3->enable();
+    }
+    
+    sprintf(bufferCommand,"%s %d","POWER",iSW);    
+    sendString(bufferCommand);
+    
+  }
+  
+  if (command=="INC"){
+    
+    string s_idxMotor = parser.next();    
+    string s_step = parser.next();    
+    int i_idxMotor=atoi(s_idxMotor.c_str());    
+    int i_step=atoi(s_step.c_str());    
+    L6474 *motor;
+    switch (i_idxMotor)
+    {
+    case 0:
+      motor=motor1;
+      break;
+    case 1:
+      motor=motor2;
+      break;
+    case 2:
+      motor=motor3;
+      break;
+    }
+    int CurrX=motor->get_position();  
+    motor->set_direction(FORWARD); 
+    for (int i=0;i<i_step;i++){
+        if (PinA4==0) {
+          motor->Step();
+          wait_us(1000);
+        }
+    } 
+    int actualX=motor->get_position();
+    int CurrEle= 0;
+    sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);
+    sendString(bufferCommand);
+    
+    
+  }
+  
+  if (command=="DEC"){
+    
+    string s_idxMotor = parser.next();    
+    string s_step = parser.next();    
+    int i_idxMotor=atoi(s_idxMotor.c_str());    
+    int i_step=atoi(s_step.c_str());    
+    L6474 *motor;
+    switch (i_idxMotor)
+    {
+    case 0:
+      motor=motor1;
+      break;
+    case 1:
+      motor=motor2;
+      break;
+    case 2:
+      motor=motor3;
+      break;
+    }
+    int CurrX=motor->get_position();     
+    //if (PinA4==0) 
+    {
+      motor->set_direction(BACKWARD);  
+      for (int i=0;i<i_step;i++){
+      motor->Step();
+      wait_us(1000);
+      }
     }
+    
+    int actualX=motor->get_position();
+    int CurrEle= 0;
+    sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);    
+    sendString(bufferCommand);
+  }
+  
+  if (command=="HOME"){
+    
+    string s_idxMotor = parser.next();    
+    int i_idxMotor=atoi(s_idxMotor.c_str());    
+    L6474 *motor;
+    switch (i_idxMotor)
+    {
+    case 0:
+      motor=motor1;
+      break;
+    case 1:
+      motor=motor2;
+      break;
+    case 2:
+      motor=motor3;
+      break;
+    }
+    int CurrX=motor->get_position();      
+    motor->set_direction(FORWARD); 
+    while (true)
+    {
+        if (PinA4==0) 
+        {
+          motor->Step();
+          wait_us(1000);
+        }
+        else
+          break;
+      
+    }
+    int actualX=motor->get_position();
+    int CurrEle= 0;
+    sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);
+    sendString(bufferCommand);
+    
+    
+  }
+
+  
+  
+  if (command=="STEP_MODE"){
+    
+    string s_step = parser.next();    
+    
+    StepperMotor::step_mode_t mode=StepperMotor::STEP_MODE_UNKNOWN ;
+    
+    if (s_step.compare("Full")==0){
+       mode=StepperMotor::STEP_MODE_FULL;
+    }
+    else if (s_step.compare("Half")==0){
+      mode=StepperMotor::STEP_MODE_HALF;
+    }
+    else if (s_step.compare("1/4")==0){
+      mode=StepperMotor::STEP_MODE_1_4;
+    }
+    else if (s_step.compare("1/8")==0){
+      mode=StepperMotor::STEP_MODE_1_8;
+    }
+    else if (s_step.compare("1/16")==0){
+      mode=StepperMotor::STEP_MODE_1_16;
+    }
+    if (mode!=StepperMotor::STEP_MODE_UNKNOWN)
+    {
+      motor1->set_step_mode(mode);
+      motor2->set_step_mode(mode);
+      motor3->set_step_mode(mode);
+      motor2->enable();
+      motor3->enable();
+      sprintf(bufferCommand,"%s %s","OK",line.c_str());    
+      sendString(bufferCommand);
+    }
+    else
+    {
+      sprintf(bufferCommand,"%s %s","KO",line.c_str());    
+      sendString(bufferCommand);
+    }
+    
+    
+    
+    
+    
+  }
+  
+  if (command=="GOTOA"){
+    
+    int CurrX=motor2->get_position();         
+    string TO = parser.next();    
+    int iTo=atoi(TO.c_str());    
+    int diff=abs(iTo-CurrX);    
+    if (CurrX>iTo)
+    {
+      motor2->set_direction(BACKWARD);  
+      while (true)
+      {
+        if(diff==0) break;
+        if (PinA3==0) break;    
+        motor2->Step();
+        diff=diff-1;
+      }   
+    }
+    else if (iTo>CurrX)
+    {
+      motor2->set_direction(FORWARD);  
+      while (true)
+      {
+        if(diff==0) break;
+        if (PinA4==0) break;    
+        motor2->Step();
+        diff=diff-1;
+      }   
+      
+    }    
+    
+    int actualX=motor2->get_position();
+    unsigned int CurrEle= 0;
+    sprintf(bufferCommand,"%s=%d,%d\n","POSX",actualX,CurrEle);    
+    sendString(bufferCommand);
+    
+  }
+  
+  
+  
+  
+  
+  
 }
 
+
+
+/**
+* @brief  This is an example of error handler.
+* @param[in] error Number of the error
+* @retval None
+* @note   If needed, implement it, and then attach it:
+*           + motor->attach_error_handler(&my_error_handler);
+*/
+void my_error_handler(uint16_t error)
+{
+  /* Printing to the console. */
+  printf("Error %d detected\r\n\n", error);
+  
+  /* Infinite loop */
+  
+}
+
+
+/* Main ----------------------------------------------------------------------*/
+
+
+char * debug_r;
 int main()
 {
-    printf("\n\n*** RTOS basic example ***\n");
-
-    thread.start(print_thread);
+  
+  
+  sendString("INFO INT WITH POWER OFF"); 
+  POWER=1;
+  wait_ms(4000);
+  
+  
+  
+  /*----- Initialization. -----*/
+  
+  /* Initializing SPI bus. */
+  DevSPI dev_spi(D11, D12, D13);
+  
+  /* Initializing Motor Control Components. */
+  motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
+#ifdef M2
+  motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
+#ifdef M3
+  motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
+#endif
+#endif
+  
+  motor1->attach_error_handler(my_error_handler);
+  
+  //motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
+  //    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
+  //    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
+  
+  
+  if (motor1->init() != COMPONENT_OK) {
+    exit(EXIT_FAILURE);
+  }
+#ifdef M2    
+  if (motor2->init() != COMPONENT_OK) {
+    exit(EXIT_FAILURE);
+  }
+#ifdef M3
+  if (motor3->init() != COMPONENT_OK) {
+    exit(EXIT_FAILURE);
+  }
+#endif
+#endif
+  
+  
+  motorStepMode_t stepm;
+  step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_4);
+#ifdef M2    
+  step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_8);
+#ifdef M3    
+  step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_16);
+#endif
+#endif
+  
+  step_mode=motor1->get_parameter(L6474_STEP_MODE);
+#ifdef M2    
+  step_mode=motor2->get_parameter(L6474_STEP_MODE);
+#ifdef M3    
+  step_mode=motor3->get_parameter(L6474_STEP_MODE);
+#endif
+#endif
+  
+  /* Printing to the console. */
+  
+  
+  InterA4.rise(&alarm_pinA4_on);
+  InterA4.fall(&alarm_pinA4_off);
+  InterA4.enable_irq();
+  
+  
+  motor2->enable();
+  motor3->enable();
+  
+  
+  
+  
+  while(false) {
+    motor2->set_direction(FORWARD);  
+    motor3->set_direction(FORWARD);
+    for (int i=0;i<200*16;i++){
+      motor2->Step();      
+      motor3->Step();      
+      wait_us(300);
+    }
+    
+    POWER=0;
+    wait_ms(5000);
+    POWER=1;
+    wait_ms(5000);
+    
+    motor2->enable();
+    motor3->enable();
+    
+    motor2->set_direction(BACKWARD);  
+    motor3->set_direction(BACKWARD);
+    
+    for (int i=0;i<200*16;i++){
+      motor2->Step();      
+      motor3->Step();      
+      wait_us(300);
+    }
+    
+    POWER=0;
+    wait_ms(5000);
+    POWER=1;
+    wait_ms(5000);
+    
+    motor2->enable();
+    motor3->enable();
+    
+  }
+  
+  /* Infinite Loop. */
+  while(false) {
+    
+    step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_4);
+#ifdef M2    
+    step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_4);
+#ifdef M3    
+    step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_4);
+#endif
+#endif
+    
+    /* Requesting to go to a specified position. */
+    motor1->go_to(- (STEPS >> 1));
+#ifdef M2    
+    motor2->go_to(- (STEPS >> 1));
+#ifdef M3    
+    motor3->go_to(- (STEPS >> 1));
+#endif
+#endif
+    
+    /* Waiting while the motor is active. */
+    motor1->wait_while_active();
+#ifdef M2    
+    motor2->wait_while_active();
+#ifdef M3    
+    motor3->wait_while_active();
+#endif
+#endif
+    
+    wait_ms(1000);
+    
+    step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_8);
+#ifdef M2    
+    step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_16);
+#ifdef M3    
+    step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_4);
+#endif
+#endif
+    /* Requesting to go to a specified position. */
+    motor1->go_to( (STEPS >> 1));
+#ifdef M2    
+    motor2->go_to( (STEPS >> 1));
+#ifdef M3    
+    motor3->go_to( (STEPS >> 1));
+#endif
+#endif
+    
+    /* Waiting while the motor is active. */
+    motor1->wait_while_active();
+#ifdef M2    
+    motor2->wait_while_active();
+#ifdef M3    
+    motor3->wait_while_active();
+#endif
+#endif
+    
+    
+  }
+  
+  string line;
+  
+  
+  
+  
+  while(true){
+    
+    if ( Console.readable() ) 
+    {
+      debug_r= (char*) line.c_str();
+      
+      char serial_char = Console.getc();
+      serial_count++;
+      if (serial_char == '\n' )
+      {
+        // pc.puts(line.c_str());
+        processLine(line);
+        line="";
+        serial_count=0;
+        
+      }
+      else
+      {
+        line.push_back(serial_char);
+      }
+      
+    }
+    
+  }
+}  
 
-    while (true) {
-        led1 = !led1;
-        wait(0.5);
-    }
-}