File content as of revision 4:aa82d3dbbe0f:

//Assignment 5 Accelerometer and Servo with Data Logging
//Starter code from Stephen Licht
//Created 11/9/2021

#include "mbed.h"
#include "MMA8452Q.h"
#include "SDFileSystem.h"
#include <cmath> //for fabs
#include <math.h> //for atan

//Define constants for timing of read and write:
#define READTIME 0.2 //5 Hz
#define WRITETIME 1.0 //1sec
#define STORETIME 0.2 //5 Hz
#define SERVOTIME 0.2 //5 Hz

MMA8452Q accel(p28,p27,0x1D);
Serial pc(USBTX,USBRX);
SDFileSystem sd(p5, p6, p7, p8, "sd");
PwmOut servo(p21);

DigitalOut Led1(LED1);
DigitalOut Led2(LED2);
DigitalOut Led3(LED3);

Ticker accel_reading;
Ticker file;
Ticker servo_move;

Timer serial_reporting;
Timer t;

int filecount = 0; //define count as starting at 0
int data=0; //dataCount

float pulseWidth=0; //pulse width of servo

double angle_rad=0; //angle of servo in rad
float angle_deg=0; //angle of servo in deg

float accX_g,accY_g,accZ_g;

void light();
void getAccelerometerValues();
void infostore();
void servo_set_angle(float angle);
void angle();


int main()
{
    t.start(); //start overall timer

    //Initialize accelerometer:
    accel.init();

    //Start ticker to read accelerometer at 5Hz
    accel_reading.attach(&getAccelerometerValues,READTIME);

    //Start ticker to store data at 5Hz
    file.attach(&infostore,STORETIME);

    //Start ticker to adjust servo at 5Hz
    servo_move.attach(&angle,SERVOTIME);

    //Begin serial write timer for the first time:
    serial_reporting.start();

    while(1) {
        //Use a timer to manage writing to the serial port (approximate timing):
        if(serial_reporting.read()>WRITETIME) {
            serial_reporting.reset();
            serial_reporting.start();
            pc.printf("X,Y,Z[Gs]:%5.2f,%5.2f,%5.2f\r\n",accX_g,accY_g,accZ_g);
        }
        //new file count every ten seconds or 50 data points
        if(data>=50){
            filecount=filecount+1;
            data=0;
        }
    }
}

void infostore()
{
    mkdir("/sd/mydir", 0777);
    char file_name[100];
    sprintf(file_name,"/sd/mydir/log_file_%d.txt",filecount);
    FILE *fp = fopen(file_name,"a");
    if(fp == NULL) {
        error("Could not open file for write /n");
    }

    fprintf(fp,"$ACC,%f,%f,%f,%f\r\n",t.read(),accX_g,accY_g,accZ_g);
    fprintf(fp, "$SERV,%f,%f,%f\r\n",t.read(),(pulseWidth/1000000.000)*5*100,angle_deg);
    fclose(fp);
    data = data+1;
}

void getAccelerometerValues()
{
    accX_g = accel.readX();
    accY_g = accel.readY();
    accZ_g = accel.readZ();
}

void light()
{
    float abs_accX=fabs(accX_g);
    float abs_accY=fabs(accY_g);
    float abs_accZ=fabs(accZ_g);

    if (abs_accX>abs_accY && abs_accX>abs_accZ) {
        Led1=1;
        Led2=0;
        Led3=0;
    }
    if (abs_accY>abs_accX && abs_accX>abs_accZ) {
        Led1=0;
        Led2=1;
        Led3=0;
    }
    if (abs_accZ>abs_accX && abs_accX>abs_accY) {
        Led1=0;
        Led2=0;
        Led3=1;
    }
}
void angle()
{
    angle_rad=atan((accZ_g)/(accY_g));
    angle_deg=angle_rad*180/(3.14159265359);
    servo_set_angle(angle_deg);
}

void servo_set_angle(float angle)
{
    float pulseCoeff = 10.0;
    float pulseOffset = 400;
    
    //Check to make sure commanded angle is within min-max bounds:
    /*if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }*/
    
    //pc.printf("Current angle: %f\r\n", angle);

    pulseWidth =pulseCoeff*angle +pulseOffset;
    servo.pulsewidth(pulseWidth/1000000.000);
}