1

I am trying to write code to test servo motors using an encoder. I create a file in the setup part, and then set 1500 microseconds to 0 for the encoder. However, if I try to print something to the SD card after I create the file, it will not print to the card. The file is always created but nothing is written to it. I have tested the mapping function separately and it works, but the code never reaches that point. It quits sometime after creating the mapping.txt file but before starting the loop.

// voltage to servo is divided by 3 to get to something usable for teensy

#include <Encoder.h>
#include <SD.h>
#include <SPI.h>
#define numFrame 6750

float twopi = 2.0 * PI;
float hz2rps = twopi;
float rps2hz = 1/hz2rps;

//for the encoder
// Encoder Pins for Channel A and B.
int encPin1=6;
int encPin2=7;

// Per the label on the Encoder this should be 1250 CPR, but 5000 gives actual angle (and is an option that could have been added after purchase)
int pos[numFrame]; // Array to store positions
int pw[numFrame]; // Array to store analog input of PWM


// for writitng to the SD card
File mappingandrange;
File step_file[10];
File bandwidth_file[10];
int filenum=0;
int chipSelect=10;
char buffer[10];

// Encoder
Encoder myEnc(encPin1, encPin2);

float poscmd;
int8_t poscmdInt[numFrame];

// timing variables to stabilize the frame
unsigned long start_frame;
unsigned long stop_frame;
unsigned long stopstop_frame;
unsigned long delta_frame;

int freqFrame_hz = 2000;
int timeFrame_us = 1000000/freqFrame_hz;
int writePwmPin = 5;
int readPwmPin = 2; // Analog pin to readback the PWM out
float freqWritePwm_hz = 333;
float freqPwmCenter_us = 1520.0;
float pulse = (freqWritePwm_hz / 1000000.0) * 65535.0;

float mag = 500.00;
float magScale = pow(2, 15) / mag;
float tEnd_s = 3.0;
float timeCurr_s = 0.0;
float freqStart_rps = 1 * hz2rps; // Start Frequency**
float freqEnd_rps = 30 * hz2rps; // End Frequency**
float freqK = (freqEnd_rps - freqStart_rps) / (2.0 * tEnd_s); // Parameter for Sweep

//things leftover from when i tried to write code
int freq=250;
double average;
double lengthtotalmicros;
double percent;
int analogfreq;
const int servopin=5;
int num=0;
int j=0;
int i=0;
float mapp;
double resistance=1; //resistance of resistor used to measure current though servo
int voltagepin1=1;//very high resistance bridged in parallel with servo
int voltagepin2=2;//very high resistance in parallel with servo
int voltagepin3=3;//low resistance in series with the servo

void setup()
{
  pinMode(servopin, OUTPUT);

  //setup for the sd card
  pinMode(chipSelect, OUTPUT);
  SD.begin();
  mappingandrange=SD.open("mapping.txt", FILE_WRITE);

  //initalize the servo to 1500 microseconds and the position to 0
  servowrite(1500, freqWritePwm_hz, servopin);
  delay(500);
  myEnc.write(0);



}

void loop()
{
  mapp=mapping();

  //setup to run servo_step with magnitude 10 degrees
  filenum=1;
  freqFrame_hz=2000;
  freqWritePwm_hz=333;
  mag=10.0;//magnitude in degrees
  servo_step(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp, tEnd_s);

  //setup to run servo_step with magnitude 60 degrees
  filenum=2;
  mag=60.0;//magnitude in degrees
  servo_step(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp, tEnd_s);

  //then do setup for the servo_bandwidth function. 
   freqFrame_hz = 151; // Command frame rate **** how fast teensy loop runs
   timeFrame_us = 1000000/freqFrame_hz;
   writePwmPin = 5; // PWM write pin # ****servo input pin
   freqWritePwm_hz = 333; // PWM rate **** pwm pulse frequency
   freqPwmCenter_us = 1520.0; // PWM center frequency
   pulse = (freqWritePwm_hz / 1000000.0) * 65535.0; // Scaling for pulse width duty cycle

   mag = 40.0; // This is the PWM magnitude, need to convert from angle to PWM **** need to later convert from degrees using mapping
   magScale = pow(2, 7) / mag; // Scalling so that PWM change can be logged as int8
   tEnd_s = numFrame / freqFrame_hz; // End Time** three below possibly part of function call and change run to run
   timeCurr_s; // Current Time
   freqStart_rps = 1 * hz2rps; // Start Frequency**
   freqEnd_rps = 30 * hz2rps; // End Frequency**
   freqK = (freqEnd_rps - freqStart_rps) / (2.0 * tEnd_s); // Parameter for Sweep

   mag=1;
   filenum=1;
   servo_bandwidth(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp,tEnd_s, freqStart_rps, freqEnd_rps);
   mag=3;
   filenum=2;
   servo_bandwidth(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp,tEnd_s, freqStart_rps, freqEnd_rps);
   mag=5;
   filenum=3;
   servo_bandwidth(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp,tEnd_s, freqStart_rps, freqEnd_rps);
   mag=10;
   filenum=4;
   servo_bandwidth(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp,tEnd_s, freqStart_rps, freqEnd_rps);
   mag=20;
   filenum=5;
   servo_bandwidth(filenum, freqFrame_hz, writePwmPin, freqWritePwm_hz, mag, mapp,tEnd_s, freqStart_rps, freqEnd_rps);


for( ; ; )
{
}

}

float mapping()
{

  servowrite(1200, freqWritePwm_hz, servopin);
  delay(250);
  pos[0]=myEnc.read();
  j=1300;
  for(int i=1; i<7; i++)
  {
    servowrite(j, freqWritePwm_hz, servopin);
    delay(200);
    pos[i]=myEnc.read();
    j=j+100;
    mappingandrange.println(pos[i]);
  }
  average=0;
  for(i=0; i<7; i++)
  {
    average=average+pos[i];
    pos[i]=0;
  }
  average=average/i;
  mappingandrange.print("The average number of positions per microsecond lengthened is ");
  mappingandrange.println(average);
  mappingandrange.print("Which corresponds to ");
  mappingandrange.print((average/5000.0)*360.0);
  mappingandrange.print("degrees per microsecond lengthened, or ");
  mappingandrange.print((average/5000.0)*3600.0);
  mappingandrange.println(" degrees per 100 microsecond change");  

  mapp=(average/5000.0)*360.0;
  mappingandrange.close();
  return mapp;
}




void servowrite(int widthmicros, int freq, int servopin)
{
  analogWriteFrequency(servopin, freq);
  lengthtotalmicros=(1.0/freq)*1000000.0;
  percent=widthmicros/lengthtotalmicros;
  analogfreq=percent*256.0; // changes how much of the duty cycle is up from 0 to 256
  analogWrite(servopin, analogfreq);
}

//input the magnitude in degrees and then the function will convert them to pwm values necessary 
void servo_step(int filenum, int freqFrame_hz, int writePwmPin, float freqWritePwm_hz, float mag, float mapping, float tEnd_s)
{
    //sd assumes that pinMode(chipSelect, OUTPUT) and SD.begin, and file has been declared
  sprintf(buffer, "servo_step %d.txt", filenum);
  step_file[filenum]=SD.open(buffer, FILE_WRITE);
  step_file[filenum].println("Frequency sweep for the given parameters");
  step_file[filenum].print("Command frame rate: ");
  step_file[filenum].println(freqFrame_hz);
  step_file[filenum].print("PWM pulse frequency: ");
  step_file[filenum].println(freqWritePwm_hz);
  step_file[filenum].print("The magnitude of the sweep in PWM microseconds is: ");
  step_file[filenum].println(mag/mapping);
  step_file[filenum].print("This corresponds to a magnitude in degrees of: ");
  step_file[filenum].println(mag);

  //converting degrees magnitude to pwm magnitude
  mag=mag/mapping;


  analogWriteResolution(16); // Setting the write resolution for PWM
  analogWriteFrequency(writePwmPin, freqWritePwm_hz); // Setting the frequency for the PWM update rate
  analogWrite(writePwmPin, freqPwmCenter_us * pulse); // sending the servo to neutral

  analogReadRes(16); // set A/D converter to 16 bit

  delay(1000); // waiting for user to pull up debug serial channel

  //end of setup part of function now moving on to the loop part of function which only actually runs once anyway
  //should run through the frequewncy sweep and print the data to the sd card for the given parameters

  delay(1000); // waiting for starting the test

  poscmd = 0.0;
  int i = 0;
  while(timeCurr_s < tEnd_s){
    start_frame = micros(); // get the start of the frame

    timeCurr_s = (float)(i * timeFrame_us) / 1000000.0;
    if(timeCurr_s > 1 & timeCurr_s <= 2)
    {
      poscmd = mag;
    }
    else if(timeCurr_s > 2)
    {
      poscmd = 0.0;
    }

    poscmdInt[i] = (int)(poscmd/magScale);

    analogWrite(writePwmPin,(freqPwmCenter_us + poscmd) * pulse); // write servo command
    pw[i] = analogRead(readPwmPin); // read the pwm signal
    pos[i] = myEnc.read();

    i++;

    stop_frame = micros(); // get the end of the frame
    delta_frame = stop_frame - start_frame;
    if(delta_frame < timeFrame_us){
      delayMicroseconds(timeFrame_us - delta_frame); // sleeping to stabilize the frame at 100 Hz
    }
  }

  delay(1000); // wait to get the serial monitor open
  for(int j = 0; j < i; j++){ // printing out the stored data so we can import to MATLAB
    step_file[filenum].print(pos[j]);
    step_file[filenum].print("\t");
    step_file[filenum].print(freqPwmCenter_us + (float)(poscmdInt[j]*magScale));
    step_file[filenum].print("\t");
    step_file[filenum].println((float)(pw[j]));

  }
  step_file[filenum].close();
}



void servo_bandwidth(int filenum, int freqFrame_hz, int writePwmPin, float freqWritePwm_hz, float mag, float mapping, float tEnd_s, float freqStart_rps, float freqEnd_rps)
{
    // assumes that pinMode(chipSelect, OUTPUT) and SD.begin();
  sprintf(buffer, "servo_bandwidth %d.txt", filenum);
  bandwidth_file[filenum]=SD.open(buffer, FILE_WRITE);
  bandwidth_file[filenum]=SD.open(buffer, FILE_WRITE);
  bandwidth_file[filenum].println("Frequency sweep for the given parameters");
  bandwidth_file[filenum].print("Command frame rate: ");
  bandwidth_file[filenum].println(freqFrame_hz);
  bandwidth_file[filenum].print("PWM pulse frequency: ");
  bandwidth_file[filenum].println(freqWritePwm_hz);
  bandwidth_file[filenum].print("The magnitude of the sweep in PWM microseconds is: ");
  bandwidth_file[filenum].println(mag/mapping);
  bandwidth_file[filenum].print("This corresponds to a magnitude in degrees of: ");
  bandwidth_file[filenum].println(mag);
  bandwidth_file[filenum].print("The starting frequency is: ");
  bandwidth_file[filenum].println(freqStart_rps);
  bandwidth_file[filenum].print("The ending frequency is: ");
  bandwidth_file[filenum].println(freqEnd_rps);

  mag=mag/mapping;



  analogWriteResolution(16); // Setting the write resolution for PWM
  analogWriteFrequency(writePwmPin, freqWritePwm_hz); // Setting the frequency for the PWM update rate
  analogWrite(writePwmPin, freqPwmCenter_us * pulse); // sending the servo to neutral
  delay(1000); // waiting for user to pull up debug serial channel

  // loop part of function below
    delay(1000); // waiting for starting the test

  for(int i = 0; i < numFrame; i++){
    start_frame = micros(); // get the start of the frame

    timeCurr_s = (float)(i * timeFrame_us) / 1000000.0; // Current time
    poscmd = mag * sin(freqStart_rps * timeCurr_s + freqK * timeCurr_s * timeCurr_s); // Sweep signal
    poscmdInt[i] = (int)(poscmd*magScale); // integer conversion for logging
    analogWrite(writePwmPin,(freqPwmCenter_us + poscmd) * pulse); // write servo command
    pos[i] = myEnc.read(); // read encoder value

    stop_frame = micros(); // get the end of the frame
    delta_frame = stop_frame - start_frame;
    if(delta_frame < timeFrame_us){
      delayMicroseconds(timeFrame_us - delta_frame); // sleeping to stabilize the frame
    }
  }
  //delay(1000); // wait to open the serial monitor
  for(int j = 0; j < numFrame; j++){ // printing out the stored data
    bandwidth_file[filenum].print(pos[j]);
    bandwidth_file[filenum].print("\t");
    bandwidth_file[filenum].println(freqPwmCenter_us + (float)(poscmdInt[j]/magScale));
  }
  bandwidth_file[filenum].close();
}
  • 2
    You can't really expect people to wade through over 300 lines of poorly formatted code to find your bugs. At the very least, you should have isolated the part of the code you suspect is wrong, as well as say what individual tests you run to ensure it's not some piece of hardware that's failing. – JayEye Jun 13 '16 at 21:09
  • 2
    Its very difficult to try and guess what is happening in your code. Why have you declared the FILE variables at global scope when they could be within the functions? If your code isn't executing mapping(), but is opening the file, then one of the last three lines of setup has to be the caused, can you focus on that, add Serial.Print to the code to see where it is dying. – Code Gorilla Jun 14 '16 at 9:53
  • Literal buffer overflow in servo_step. You define it as array of 10 chars waaay up top, then attempt to put more than 14 chars via sprintf in servo_step. Bad Things™ happen thusly. May be other issues, but that one is bad enough to cause failure. – lornix Apr 11 '17 at 16:54
  • On first view: You open the SD-Card file but you don't close it in setup. I am not sure if that will work. When I use the SD card I open the file, write, close the file, and continue with other steps. If any "external" value is written to the card then I save that value to a variable before I open the file to make sure when the SD card is busy nothing else is used. – Edgar Jun 29 '19 at 2:57

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.