Eric Beauchemin
commit
Dependencies: PID SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed millis
Diff: main.cpp
- Revision:
- 0:3a7cef09ad54
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 29 14:57:55 2016 +0000
@@ -0,0 +1,515 @@
+#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)
+
+}
+
+