File content as of revision 1:d6183c8b01de:

#include "mbed.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include "SDFileSystem.h"
#include "StepperMotor_X27168.h"
#include <mbed.h>
#include <mpr121.h>
#include "rtos.h"
#include "PID.h"
#include "millis/millis.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
SDFileSystem sd(p5, p6, p7, p8, "sd");

float xNowPrint;
float yNowPrint;
float zNowPrint;

#define  DEFAULT_Kp 5//mess with these to increase speed, was 1, .002, 20
#define  DEFAULT_Ki 0.01
#define  DEFAULT_Kd 20
#define THERMISTORNOMINAL 100000      // 100k 
// temp. for nominal resistance (almost always 25 C)
#define TEMPERATURENOMINAL 25
// The beta coefficient of the thermistor (usually 3000-4000)
#define BCOEFFICIENT 3950
// the value of the 'other' resistor
#define SERIESRESISTOR 4700
#define AUTOMATIC 1
#define MANUAL    0
#define DIRECT  0
#define REVERSE  1
#define thermistor A3                       // FRDM-K64F Analog input pin A3   - Adjust to your particular board
#define driver PTC3                         // FRDM-K64F PWM output pin PTC3   - Adjust to your particular board

AnalogIn Thermistor(p20);//thermistor);            // Read temperature value from thermistor on A3
PwmOut Driver(p25);                      // PWM drive FET heater on PTC3  values are 0-1.0
// For 0-100%

float Input, Output, Setpoint, currTemp =0;
PID controller(&Input, &Output, &Setpoint, DEFAULT_Kp , DEFAULT_Ki , DEFAULT_Kd , DIRECT);

#define RATE 1.0                            // Print rate  Once per second

float scaleX = .05;
float scaleY = .1;
float scaleZ = .1;

StepperMotor_X27168 smotor1(p23, p24, p22, p21);
StepperMotor_X27168 smotor2(p9, p10, p29, p30);
StepperMotor_X27168 smotor3(p14, p15, p13, p12);
StepperMotor_X27168 smotor4(p17, p16, p18, p19);

// Setup the i2c bus on pins 9 and 10
I2C i2c(p28, p27);
// Setup the Mpr121:
// constructor(i2c object, i2c address of the mpr121)
Mpr121 mpr121(&i2c, Mpr121::ADD_VSS);

enum moveMotor { NONE, XPLUS, XMINUS, YPLUS, YMINUS, ZPLUS, ZMINUS };
moveMotor motorMove = NONE;

//all motors initially stopped
int motor1Go = 2;
int motor2Go = 2;
int motor3Go = 2;
int motor4Go = 2;

char *strdup (const char *s)
{
    char *d = (char*)malloc (strlen (s) + 1);   // Space for length plus nul
    if (d == NULL) return NULL;          // No memory
    strcpy (d,s);                        // Copy the characters
    return d;                            // Return the new string
}

//split a string on a delimiter
char** str_split(char* a_str, const char a_delim)
{
    char** result    = 0;
    size_t count     = 0;
    char* tmp        = a_str;
    char* last_comma = 0;
    char delim[2];
    delim[0] = a_delim;
    delim[1] = 0;

    /* Count how many elements will be extracted. */
    while (*tmp) {
        if (a_delim == *tmp) {
            count++;
            last_comma = tmp;
        }
        tmp++;
    }

    /* Add space for trailing token. */
    count += last_comma < (a_str + strlen(a_str) - 1);

    /* Add space for terminating null string so caller
       knows where the list of returned strings ends. */
    count++;

    result = (char**)malloc(sizeof(char*) * count);

    if (result) {
        size_t idx  = 0;
        char* token = strtok(a_str, delim);

        while (token) {
            assert(idx < count);
            *(result + idx++) = strdup(token);
            token = strtok(0, delim);
        }
        assert(idx == count - 1);
        *(result + idx) = 0;
    }

    return result;
}
void motorXYprint()
{
    //smotor1.set_speed(xNowPrint/yNowPrint*200);
    int spinvar = 0;
    float xSpot = 0;
    float ySpot = 0;
    xNowPrint = floor(xNowPrint);
    yNowPrint = floor(yNowPrint);
    float product = abs(xNowPrint*yNowPrint);

    if ((xNowPrint > 0) and (xNowPrint > 0)) {
        while(spinvar <= product) {
            spinvar ++;
            if (xSpot < spinvar) {
                smotor1.step(0);
                xSpot = xSpot + product/yNowPrint;
            }
            if (ySpot < spinvar) {
                smotor2.step(0);
                ySpot = ySpot + product/xNowPrint;
            }
        }
    } else if ((xNowPrint > 0)and(xNowPrint < 0)) {
        while(spinvar <= product) {
            spinvar ++;
            if (xSpot < spinvar) {
                smotor1.step(0);
                xSpot = xSpot + abs(product/yNowPrint);
            }
            if (ySpot < spinvar) {
                smotor2.step(1);
                ySpot = ySpot + abs(product/xNowPrint);
            }
            //Thread::wait(50.0);
        }
    } else if ((xNowPrint < 0)and(xNowPrint > 0)) {
        while(spinvar <= product) {
            spinvar ++;
            if (xSpot < spinvar) {
                smotor1.step(1);
                xSpot = xSpot + abs(product/yNowPrint);
            }
            if (ySpot < spinvar) {
                smotor2.step(0);
                ySpot = ySpot + abs(product/xNowPrint);
            }
            //Thread::wait(50.0);
        }
    } else if ((xNowPrint < 0)and(xNowPrint < 0)) {
        while(spinvar <= product) {
            spinvar ++;
            if (xSpot < spinvar) {
                smotor1.step(1);
                xSpot = xSpot - (product/yNowPrint);
            }
            if (ySpot < spinvar) {
                smotor2.step(1);
                ySpot = ySpot - (product/xNowPrint);
            }
            //Thread::wait(50.0);
        }
    }
    smotor1.step(2);
    smotor2.step(2);
}

void motorZprint()
{
    //smotor2.set_speed(yNowPrint/xNowPrint*200);
    int spinvar2 = 0;
    if (zNowPrint > 0) {
        while(spinvar2 <= zNowPrint) {
            spinvar2 ++;
            //smotor3.step(0);
            Thread::wait(50.0);
        }
    } else if (zNowPrint < 0) {
        while(spinvar2 >= zNowPrint) {
            spinvar2 --;
            //smotor3.step(1);
            Thread::wait(50.0);
        }
    }
    smotor3.step(2);
}
void print()    //read from gcode file
{
    FILE *fp = fopen("/sd/text/gCode.txt", "r");
    pc.printf("Hello World!\n");
    //FILE *stream;
    char line[200];
    char g[2]="G";
    char m[2]="M";
    char x[2]="X";
    char y[2]="Y";
    char z[2]="Z";
    char e[2]="E";
    char f[2]="F";
    char** tokens;
    char** astrktokens;
    float extruderX = 0;
    float extruderY = 0;
    float extruderZ = 0;
    float extruderE = 0;
    float extruderF = 0;
    pc.printf("here\n");
    //end the 4 motor threads
    motor1Go=3;
    motor2Go=3;
    motor3Go=3;
    motor4Go=3;
    while (fgets(line, 200, fp)) { //get a line
        wait(.5);
        char firstLet[sizeof(line)];
        strncpy(firstLet, line,1);
        firstLet[1] = 0;
        if (strcmp(firstLet,g) * strcmp(firstLet,m) == 0) {
            astrktokens = str_split(line, ';');
            char* tempdata = *(astrktokens);
            //pc.printf("Temp data: %s", tempdata);
            char fullData[100];
            //char singleVariable[100];
            strcpy(fullData, tempdata);
            tokens = str_split(tempdata, ' ');
            if (tokens) {

                int i = 0;
                //ssize_t
                char* data = *(tokens + i);
                pc.printf("Data: %s ", data);
                wait(.5);
                if (strcmp(data,"G0") * strcmp(data,"G1") == 0) {
                    //Move: X,Y,Z-postions to move to, E-extrude amount, F-feedrate
                    pc.printf("Move to: ");
                    i = 1;
                    float newX = 0;
                    float newY = 0;
                    float newZ = 0;
                    float newE = 0;
                    float newF = 0;
                    while(strcmp(*(tokens + i),"\0") != 0) {
                        data = *(tokens + i);
                        //strcpy(singleVariable, data);
                        //pc.printf("Deep Data: %s ", data);
                        char firstLet[sizeof(data)];
                        strncpy(firstLet, data,1);
                        firstLet[1] = 0;
                        if (strcmp(firstLet,x) == 0) {
                            memmove(data, data+1, strlen(data));
                            newX = strtof(data, NULL);
                            pc.printf("X: %f", newX);
                            if (extruderX == 0) {
                                extruderX = newX;
                            }
                        } else if (strcmp(firstLet,y) == 0) {
                            memmove(data, data+1, strlen(data));
                            newY = strtof(data, NULL);
                            pc.printf("Y: %f", newY);
                            if (extruderY == 0) {
                                extruderY = newY;
                            }
                        } else if (strcmp(firstLet,z) == 0) {
                            memmove(data, data+1, strlen(data));
                            newZ = strtof(data, NULL);
                            pc.printf("Z: %f", newZ);
                            if (extruderZ == 0) {
                                extruderZ = newZ;
                            }
                        } else if (strcmp(firstLet,e) == 0) {
                            memmove(data, data+1, strlen(data));
                            newE = strtof(data, NULL);
                            pc.printf("E: %f", newE);
                        } else if (strcmp(firstLet,f) == 0) {
                            memmove(data, data+1, strlen(data));
                            newF = strtof(data, NULL);
                            pc.printf("F: %f", newF);
                        }
                        newX = newX-extruderX;
                        newY = newY-extruderY;
                        newZ = newZ-extruderZ;
                        xNowPrint = newX*scaleX;
                        yNowPrint = newY*scaleY;
                        zNowPrint = newZ*scaleZ;
                        if ((xNowPrint != 0) and (yNowPrint != 0)) {
                            pc.printf("xNow: %f", xNowPrint);
                            pc.printf("yNow: %f", yNowPrint);
                            motorXYprint();
                        }
                        if (zNowPrint != 0) {
                            pc.printf("yNow: %f", yNowPrint);
                            motorZprint();
                        }


                        i = i+1;
                    }
                }
            }
        }
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
    }
    fclose(fp);
    return;
}


void motor1Thread(void const *args)
{
    smotor1.set_speed(80);
    int spinvar = 0;
    while(motor1Go<3) { //3 stops the thread
        while(motor1Go != 2) {
            spinvar = 0;
            if (motor1Go == 0) {
                smotor1.step(0);

            } else if(motor1Go == 1) {
                smotor1.step(1);
            }
        }
        smotor1.step(2);
        Thread::wait(50.0);
    }
}

void motor2Thread(void const *args)
{
    smotor2.set_speed(80);
    int spinvar2 = 0;
    while(motor2Go<3) { //3 stops the thread
        while(motor2Go != 2) {
            spinvar2 = 0;
            if (motor2Go == 0) {
                smotor2.step(0);
            } else if(motor2Go == 1) {
                smotor2.step(1);
            }
        }
        smotor2.step(2);
        Thread::wait(50.0);
    }
}
void motor3Thread(void const *args)
{
    smotor3.set_speed(80);
    int spinvar3 = 0;
    while(motor3Go<3) { //3 stops the thread
        while(motor3Go != 2) {
            spinvar3 = 0;
            if (motor3Go == 0) {
                smotor3.step(0);

            } else if(motor3Go == 1) {
                smotor3.step(1);
            }
        }
        smotor3.step(2);
        Thread::wait(50.0);
    }
}
void motor4Thread(void const *args)
{
    smotor4.set_speed(80); //unnecessary but oh well
    int spinvar4 = 0;
    while(motor4Go<3) {
        while(motor4Go != 2) {
            spinvar4 = 0;
            if (motor4Go == 0) {
                smotor4.step(0);
            } else if(motor4Go == 1) {
                smotor4.step(1);
            }
        }
        smotor4.step(2);
        Thread::wait(50.0);
    }
}

void getTemperature(void const *args)
{
// This routine calculates the temperature
// using the Steinhart-Hart equation for thermistors
// https://en.wikipedia.org/wiki/Steinhart%E2%80%93Hart_equation
    float temperature, resistance;
    float steinhart;
    int a;
    Setpoint = 200;                            // Set target temperature in degrees Celcius.
    controller.SetMode(AUTOMATIC);            // Turn PID controller on.
    while(1) {
        controller.Compute();                  // Process PID loop.
        //Driver = Output/1000;                  // Sent PWM value scaled 0 - 1.0 as mbed requires ***originally divided by 1000
        if (Input < 270) {
            Driver = .9;
        } else {
            Driver = 0;
        }
        a = Thermistor.read_u16();       // Read 16bit Analog value
        //    pc.printf("Raw Analog Value for Thermistor = %d\r\n",a);
        /* Calculate the resistance of the thermistor from analog votage read. */
        resistance = (float) SERIESRESISTOR / ((65536.0 / a) - 1);
        //    pc.printf("Resistance for Thermistor = %f\r\n",resistance);

        steinhart = resistance / THERMISTORNOMINAL;         // (R/Ro)
        steinhart = log(steinhart);                         // ln(R/Ro)
        steinhart /= BCOEFFICIENT;                          // 1/B * ln(R/Ro)
        steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15);   // + (1/To)
        steinhart = 1.0 / steinhart;                        // Invert
        temperature = steinhart - 273.15;                   // convert to C
        //    pc.printf("Extruder Temperature is %f\r\n", temperature);
        Input = temperature;
        pc.printf("Input      Output     Setpoint   Kp        Ki        Kd        time\r\n");
        pc.printf("%f, %f, %f, %f, %f, %f, %d \r\n",  temperature, Output, Setpoint, controller.GetKp() , controller.GetKi() , controller.GetKd() , millis() );
        Thread::wait(1000);
    }

}
int main()
{

    int x = 0;
    int key_code=0;
    int i=0;
    int value;
    startMillis();                 // Initialize timer.
    //terminal prints status data
    pc.baud(115200);
    pc.printf("\r\nThermistor PID Test - Build " __DATE__ " " __TIME__ "\r\n");
    //motors 1-3 are x,y,or z
    Thread thread1(motor1Thread);
    Thread thread2(motor2Thread);
    Thread thread3(motor3Thread);
    //motor 4 is the feeder motor
    Thread thread4(motor4Thread);
    while(1) {
        //use keypad for controlling motors and temperature while debugging
        value=mpr121.read(0x00);
        value +=mpr121.read(0x01)<<8;
        if((value>>0)&0x01) { //zero button pressed
            myled = 1;
            motor1Go = 0; //x motor move back
        } else if((value>>1)&0x01) { //one button pressed
            myled = 0;
            motor1Go = 1; //x motor move forwards
        } else {
            motor1Go = 2;  //x motor stop
        }
        if(((value>>2)&0x01)==1) { //two button pressed
            myled = 1;
            motor2Go = 0; //y motor move backward
        } else if(((value>>3)&0x01)==1) { //three button pressed
            myled = 0;
            motor2Go = 1; //y motor forward
        } else {
            motor2Go = 2;   //else stop y motor
        }//end if
        if(((value>>4)&0x01)==1) { //four button pressed
            myled = 1;
            motor3Go = 0;   //z motor back
        } else if(((value>>5)&0x01)==1) {
            myled = 0;
            motor3Go = 1;   //z motor forward
        } else {
            motor3Go = 2;  //else stop z motor
        }//end if
        if((value>>6)&0x01) { //6 button pressed
            myled = 1;
            motor4Go = 0;
        } else if((value>>7)&0x01) { //7 button pressed
            myled = 0;
            motor4Go = 1;  //motor 4 forwards
        } else {
            motor4Go = 2;  //stop motor 4
        }//end if
        if(((value>>8)&0x01)==1) { //8 button pressed
            myled = !myled;
            Setpoint +=1;  //increase set temp (C) by 1
        } else if(((value>>9)&0x01)==1) { //button 9 pressed
            myled = !myled;
            Setpoint -= 1;  //decrease set point by 1
        }
        if((value>>10)&0x01) { //10 button pressed
            myled = 1;
            print();
        }
        Thread::wait(100);
    }//end while(1)

}