Problem with Timer function used for speed sensors (one DC motor not working)

Projects Programming
41

Ok so I got one same problem that on the beggining: only one motor working.
And second problem: txt file is corrupted and cannot be oppened, but I guess that is caused by sd card module which I have to change to a better one.
Whole code in the attachment and here:

#include <SPI.h>
#include <SD.h>

#include <AFMotor.h>
#include "SR04.h"
#include <IRremote.h>

//Ustawienia karty SD
File myFile;
const int8_t DISABLE_CHIP_SELECT = 10;

int pinCS = 53; // Pin komunikacji z karta SD

bool timeToWrite = false;

// This is a structure to hold data.
struct robotLog
{
  uint8_t leftEngine;
  uint8_t rightEngine;
  byte Direction;
};

typedef struct robotLog RobotLog;
RobotLog rbl;

// Now create the buffer. The size can be changed
const unsigned int numOfLogsToBuffer = 60;
RobotLog sdBuffer[numOfLogsToBuffer];

// index to indicate the current position in the buffer.
unsigned int logBufferIndex = 0;

//0-straight 1-turn
byte driving_status = 0;

//Ustawienie sensorow
int LEFT_SENSOR_TRIG = A2;
int LEFT_SENSOR_ECHO = A3;
int FRONT_SENSOR_TRIG = A0;
int FRONT_SENSOR_ECHO = A1;

SR04 sr04_left = SR04(LEFT_SENSOR_ECHO, LEFT_SENSOR_TRIG);
SR04 sr04_front = SR04(FRONT_SENSOR_ECHO, FRONT_SENSOR_TRIG);

//Ustawienie pilota
int RECV_PIN = 21; //pin 21

IRrecv irrecv(RECV_PIN);
decode_results results;

//Ustawienie silnikow
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(3, MOTOR34_1KHZ);

int NO_OBSTACLE = 0;
int OBSTACLE = 1;

//Zmienne PID
const float Kp_wall = 8; // stala uzyta do skretu
const float Ki_wall = 0.003; // stala calki uzyta do poprawy bledu
const float Kd_wall = 3; // stala rozniczki uzyta do poprawy predkosci poprawy bledu

const float Kp_front = 30; // stala uzyta do skretu
const float Ki_front = 0; // stala calki uzyta do poprawy bledu
const float Kd_front = 15; // stala rozniczki uzyta do poprawy predkosci poprawy bledu

const int offset = 12; // docelowa odleglosc
const int max_front_distance = 16; // maksymalna odleglosc z przodu

int Tp = 128; // docelowa predkosc - target power

float error_wall = 0;
float lastError_wall = 0;
int correction_wall = 0;
float p_wall = 0;  //proporcja
float i_wall = 0;  //calka
float d_wall = 0;  //rozniczka

int Wall = 0;
int Front = 0;

float error_front = 0;
float lastError_front = 0;
float correction_front = 0;
float p_front = 0;  //proporcja
float i_front = 0;  //calka
float d_front = 0;  //rozniczka

int leftMotorSpeed = 0;
int rightMotorSpeed = 0;

int front_status = 0;

//0 - true; 1 - false
int rightTurnBegin = 0;
int leftTurnBegin = 0;
int straightLineBegin = 0;

int speedSet = 0;


void setup() {
  // Ustawienie pilota
  pinMode(RECV_PIN, INPUT);
  irrecv.enableIRIn();

  //Ustawienie czytnika kart SD
  pinMode(53, OUTPUT);

  if (!SD.begin(pinCS))
  {
    Serial.println("SD card initialization failed");
    return;
  } else
  {
    Serial.println("SD card ready");
  }

  SD.remove("DaneMapy.txt");
  myFile = SD.open("DaneMapy.txt", O_CREAT | O_APPEND | O_WRITE);

  if (myFile) {
    myFile.println("Lewy silnik cm/s, Prawy silnik cm/s");
  }
  myFile.close();

  //setting up timer 1

  cli();//stop interrupts
  //set timer1 interrupt at 1Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 15624;// = (16*10^6) / (1*1024) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS10 and CS12 bits for 1024 prescaler
  TCCR1B |= (1 << CS12) | (1 << CS10);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  sei();//allow interrupts
  //end timer setup

  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:

  if(timeToWrite == true){
    storeData();
   
  }else{
    //starts robot
    checking_status();
  }
}

void checking_status() {
  front_status = GET_FRONT_STATUS();

  if (front_status == OBSTACLE) {

    if (leftTurnBegin == 0) {
      zero_wall();
    }
    followWall_front();

    // Zmieniam flage skretu na false do nastepnego czasu az natrafi na przeszkode
    rightTurnBegin = 1;
    straightLineBegin = 0;

  }
  if (front_status != OBSTACLE) {
    followWall();
  }
}

void followWall() {
  //Kiedy robot zaczyna jechac prosto (dopoki nie ma przeszkody) zmieniam flage na true
  driving_status = 0;
  rightTurnBegin = 0;

  Wall = sr04_left.Distance();
  error_wall = Wall - offset;
  i_wall = (i_wall + error_wall);
  d_wall = (error_wall - lastError_wall);

  correction_wall = Kp_wall * error_wall + Ki_wall * i_wall + Kd_wall * d_wall;

  //jezeli error_wall jest mniejszy od 10, oznacza to ze robot jedzie obok sciany poprawnie
  if (error_wall < 10) {

    if (straightLineBegin == 0) {
      zero_wall();
    }

    if (correction_wall > 127 && correction_wall > 0) {
      correction_wall = 127;
    }

    if (correction_wall < -127 && correction_wall < 0) {
      correction_wall = -127;
    }

    rightMotorSpeed  = Tp + correction_wall;
    leftMotorSpeed = Tp - correction_wall;

    leftTurnBegin = 0;
    straightLineBegin = 1;
  } else {

    //jezeli error_wall jest wiekszy od 10, oznacza to ze robot musi skrecic w prawo

    //zeruje wartosci na poczatku skretu w lewo
    if (leftTurnBegin == 0) {
      zero_wall();
      zero_front();
    }

    //PID do skretu w lewo
    int speed = 2.5 * error_wall + 8 * d_wall;

    if (speed > 127 && speed > 0) {
      speed = 127;
    }

    if (speed < -127 && speed < 0) {
      speed = -127;
    }

    rightMotorSpeed  = Tp + (speed);
    leftMotorSpeed = Tp - (speed);

    leftTurnBegin = 1;
    straightLineBegin = 0;
  }

  motor1.setSpeed(rightMotorSpeed);
  motor1.run(FORWARD);
  motor2.setSpeed(leftMotorSpeed);
  motor2.run(FORWARD);

  lastError_wall = error_wall;
  zero_front();
}

void zero_wall(void) {
  i_wall = 0;
  d_wall = 0;
  lastError_wall = 0;
}

void zero_front(void) {
  i_front = 0;
  d_front = 0;
  lastError_front = 0;
}

void followWall_front()
{
  driving_status = 1;
  //tu umiec zczytywanie z sensora
  Front = sr04_front.Distance();
  error_front = (Front - max_front_distance);
  i_front = (i_front + error_front);
  d_front = (error_front - lastError_front);

  correction_front = Kp_front * error_front + Ki_front * i_front + Kd_front * d_front;


  if (correction_front > 127 && correction_front > 0) {
    correction_front = 127;
  }

  if (correction_front < -127 && correction_front < 0) {
    correction_front = -127;
  }

  rightMotorSpeed  = Tp + correction_front;
  leftMotorSpeed = Tp - correction_front;

  motor1.setSpeed(rightMotorSpeed);
  motor1.run(FORWARD);
  motor2.setSpeed(leftMotorSpeed);
  motor2.run(FORWARD);

  lastError_front = error_front;
}

ISR(TIMER1_COMPA_vect) {//Interrupt at freq of 1kHz
  timeToWrite = true;
}

void storeData() {
  timeToWrite = false;
  sdBuffer[logBufferIndex].leftEngine = leftMotorSpeed;
  sdBuffer[logBufferIndex].rightEngine = rightMotorSpeed;
  sdBuffer[logBufferIndex].Direction = driving_status;

  logBufferIndex++;
  if (logBufferIndex == 60) {
    //STOP MOTOR HERE UNTIL FOR LOOP FINISHED
    myFile = SD.open("DaneMapy.txt", O_CREAT | O_APPEND | O_WRITE);
    for (int i = 0; i < 60; i++) {
      if (myFile) {
        Serial.println("Writing to DaneMapy.txt");
        myFile.print(sdBuffer[i].leftEngine);
        myFile.print(" | ");
        myFile.print(sdBuffer [i].rightEngine);
        myFile.print(" | ");
        myFile.println(sdBuffer[i].Direction);
        Serial.println(i);
        Serial.println("done.");
      }
    }
    myFile.close();
    logBufferIndex = 0;
  }
}

int GET_LEFT_STATUS(void) {
  if (sr04_left.Distance() < offset)
    return OBSTACLE;
  else
    return NO_OBSTACLE;
}

int GET_FRONT_STATUS(void) {
  if (sr04_front.Distance() < max_front_distance)
    return OBSTACLE;
  else
    return NO_OBSTACLE;
}

PID_wall_poprawka.zip (4.59 KB)

42

JohnyNapalm:
Ok so I got one same problem that on the beggining: only one motor working.

Well, no surprises there - you are using Motor 1 again and that is using Timer 1. It is configured in the constructor of AF_DCmotor and you override that configuration in your setup. Use another timer for timing or reside to millis() as Robin suggested.

43

JohnyNapalm:
Ok so I got one same problem that on the beggining: only one motor working.

Why don't you make a copy of your code and then remove ALL of the code that has nothing to do with getting the motors working.

In the code in your Original Post there were two ISRs for counting pulses - they seem to have disappeared from the version in Reply #40.

There seems little point worrying about an SD Card if the basics aren't working.

...R

44 
unsigned long startMillis;
unsigned long currentMillis;
const unsigned long period = 1000;
void setup(){
}
void timerMillis(){
  if(currentMillis - startMillis >= period){
    timeToWrite = true;
    startMillis = currentMillis;
  }
}

void loop(){
  startMillis=millis();

  timerMillis();
  if(timeToWrite == true){
    storeData();
  }else{
    //starts robot
    checking_status();
  }
}

That’s the way I should do it using millis() right?

45

Robin2:
Why don't you make a copy of your code and then remove ALL of the code that has nothing to do with getting the motors working.

In the code in your Original Post there were two ISRs for counting pulses - they seem to have disappeared from the version in Reply #40.

There seems little point worrying about an SD Card if the basics aren't working.

...R

Yeah I am going to turn on speed sensors just after I figure out the problem with SD card writing