polulu.cpp

/*
   analog1.cpp
 
   g++ -o Binaries/polulu1 polulu1.cpp -lpigpio -lrt -lpthread
 
   sudo ./Binaries/polulu1

   gpioWrite(18, 0); // high
   gpioWrite(18, 1); // low

   https://github.com/chojayr/Pi-dro/blob/master/Pidro.py
*/
 
#include <stdio.h>
#include <stdlib.h>
 
#include <pigpio.h>
#include <iostream>
#include <string>
#include <cstdlib>

using namespace std;

const static int  PWMA = 17;
const static int  AIN1 = 27;
const static int  AIN2 = 4;
const static int  PWMB = 14;
const static int  BIN1 = 24;
const static int  BIN2 = 25;
const static int  STBY = 22;


void init();
void forward(unsigned int motor);
void back(unsigned int motor);
void stop(unsigned int motor);


void exit_program(int code);

int main(int argc, char *argv[])
{
   double start;
 
   if (gpioInitialise() < 0)
   {
      fprintf(stderr, "pigpio initialisation failed\n");
      return 1;
   }

   /* Set GPIO modes */
   gpioSetMode(PWMA, PI_OUTPUT);
   gpioSetMode(AIN1, PI_OUTPUT);
   gpioSetMode(AIN2, PI_OUTPUT);
   gpioSetMode(PWMB, PI_OUTPUT);
   gpioSetMode(BIN1, PI_OUTPUT);
   gpioSetMode(BIN2, PI_OUTPUT);
   gpioSetMode(STBY, PI_OUTPUT);

   // init motors
   init();

   // probram loop
   string commandBuffer;
   cout << "Usage 'motor command', 1 forward" << endl;
   while(1)
   {

      cout << ">> ";
      getline(cin, commandBuffer);

      // parse string
      if (commandBuffer.find("exit") == 0)
      {
         exit_program(0);
      }

      int motor = std::atoi(commandBuffer.substr(0,1).c_str());

      if (motor == 0)
      {
         cout << "Can not parse motor number" << endl;
         continue;
      }

      string motorCommand = commandBuffer.substr(2);
      if (motorCommand.empty())
      {
         cout << "Can not parse comand" << endl;
         continue;
      }

      if (motorCommand.find("forward") == 0)
      {
            forward(motor);
      }
      else if (motorCommand.find("back") == 0)
      {
            back(motor);
      }
      else if (motorCommand.find("stop") == 0)
      {
            stop(motor);
      }
      else
      {
        cout << "No comand found" << endl;
      }

   }

   /* Stop DMA, release resources */
   gpioTerminate();

   exit_program(0);
   return 0;
}

void init()
{
   gpioWrite(PWMA, 1);
   gpioWrite(PWMB, 1);
   gpioWrite(STBY, 1);

    gpioWrite(AIN1, 0);
   gpioWrite(AIN2, 0);

    gpioWrite(BIN1, 0);
    gpioWrite(BIN2, 0);
}

void forward(unsigned int motor)
{
   if (motor == 1)
   {
          printf("motor 1 forward \n");

      gpioWrite(AIN1, 0);
      gpioWrite(AIN2, 1);
   }
   else if (motor == 2)
   {
         printf("motor 2 forward \n");

      gpioWrite(BIN1, 0);
      gpioWrite(BIN2, 1);
   }
}


void back(unsigned int motor)
{
   if (motor == 1)
   {
          printf("motor 1 back \n");

      gpioWrite(AIN1, 1);
      gpioWrite(AIN2, 0);
   }
   else if (motor == 2)
   {
         printf("motor 2 back \n");

      gpioWrite(BIN1, 1);
      gpioWrite(BIN2, 0);
   }
}

void stop(unsigned int motor)
{
   printf("fn stop %d\n", motor);

   if (motor == 1)
   {
      gpioWrite(AIN1, 0);
      gpioWrite(AIN2, 0);
   }
   else if (motor == 2)
   {
      gpioWrite(BIN1, 0);
      gpioWrite(BIN2, 0);
   }
}

void exit_program(int code)
{
   printf("%s %d\n", "End of program with code:", code);
   exit(code);
}