GPIO Not response

edit

I install ROS Kinetic on Ubuntu core on Nanopi M1, and run this code:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include <string>
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>

// Set variables for the GPIO motor pins
const short pinMotorAForwards  = 12;      // Pin 19 on board
const short pinMotorABackwards = 13;      // Pin 21 on board
const short pinMotorBForwards  = 10;      // Pin 24 on board
const short pinMotorBBackwards = 11;      // Pin 26 on board
const short STOP = 0;

void setup();

void chatterCallback(const std_msgs::String::ConstPtr &msg);

// Turn all motors off FUNCTION DECLARATION
void stopMotors();

// Turn both motors forwards FUNCTION DECLARATION
void forwards();

// Turn both motors backwards FUNCTION DECLARATION
void backwards();

// Turn left FUNCTION DECLARATION
void left();

// Turn Right FUNCTION DECLARATION
void right();

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "motors");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("message",
        100, chatterCallback);
    ros::spin();
    return 0;
}

void chatterCallback(const std_msgs::String::ConstPtr &msg)
{
    // ROS_INFO("I heard: [%s]", msg->data.c_str());
    const char *command = msg->data.c_str();
    if(msg->data == "EXIT" || msg->data == "E"){
      ROS_INFO("I get command: [%s] - EXIT!", command);
    } else if (msg->data == "FORWARD" || msg->data == "F") {
      forwards();
      ROS_INFO("I get command: [%s] - FORWARD!", command);
    } else if (msg->data == "BACKWARD" || msg->data == "B") {
      backwards();
      ROS_INFO("I get command: [%s] - BACKWARD!", command);
    } else if (msg->data == "LEFT" || msg->data == "L") {
      left();
      ROS_INFO("I get command: [%s] - LEFT!", command);
    } else if (msg->data == "RIGHT" || msg->data == "R") {
      right();
      ROS_INFO("I get command: [%s] - RIGHT!", command);
    } else if (msg->data == "STOP" || msg->data == "S") {
      stopMotors();
      ROS_INFO("I get command: [%s] - STOP!", command);
    } else {
      ROS_INFO("I GET COMMAND THAT NOT DEFINED!!!");
    }
}

// Setup & Configure
void setup(){
  wiringPiSetup() ;
  pinMode(pinMotorAForwards,  OUTPUT);
  pinMode(pinMotorABackwards, OUTPUT);
  pinMode(pinMotorBForwards,  OUTPUT);
  pinMode(pinMotorBBackwards, OUTPUT);
}

// Turn all motors off
void stopMotors(){
  digitalWrite(pinMotorAForwards,  LOW);
  digitalWrite(pinMotorABackwards, LOW);
  digitalWrite(pinMotorBForwards,  LOW);
  digitalWrite(pinMotorBBackwards, LOW);
}

// Turn both motors forwards
void forwards(){
  digitalWrite(pinMotorAForwards,  HIGH);
  digitalWrite(pinMotorABackwards, LOW);
  digitalWrite(pinMotorBForwards,  HIGH);
  digitalWrite(pinMotorBBackwards, LOW);
  // delay (1500);
  // stopMotors();
}

// Turn both motors backwards
void backwards(){
  digitalWrite(pinMotorAForwards,  LOW);
  digitalWrite(pinMotorABackwards, HIGH);
  digitalWrite(pinMotorBForwards,  LOW);
  digitalWrite(pinMotorBBackwards, HIGH);
  // delay (1500);
  // stopMotors();
}

// Turn left
void left(){
  digitalWrite(pinMotorAForwards,  LOW);
  digitalWrite(pinMotorABackwards, HIGH);
  digitalWrite(pinMotorBForwards,  HIGH);
  digitalWrite(pinMotorBBackwards, LOW);
  // delay (1500);
  // stopMotors();
}

// Turn Right
void right(){
  digitalWrite(pinMotorAForwards,  HIGH);
  digitalWrite(pinMotorABackwards, LOW);
  digitalWrite(pinMotorBForwards,  LOW);
  digitalWrite(pinMotorBBackwards, HIGH);
  // delay (1500);
  // stopMotors();
}

ros logs work fine an both root and normal user, but nothing happen!!! Do you any suggestion to check?

ps: this code in cpp appm, work fine, but I must run as root.