![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
commit
Dependencies: PID SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed millis
Diff: main.cpp
- Revision:
- 0:3a7cef09ad54
diff -r 000000000000 -r 3a7cef09ad54 main.cpp --- /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) + +} + +