Eric Beauchemin / Mbed 2 deprecated Printer

commit

Dependencies:   PID SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed millis

main.cpp@1:d6183c8b01de, 2016-04-29 (annotated)

Committer:
ebeauchemin3
Date:
Fri Apr 29 15:00:18 2016 +0000
Revision:
1:d6183c8b01de
Parent:
0:3a7cef09ad54 
commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ebeauchemin3 0:3a7cef09ad54 1 #include "mbed.h"
ebeauchemin3 0:3a7cef09ad54 2 #include <stdio.h>
ebeauchemin3 0:3a7cef09ad54 3 #include <stdlib.h>
ebeauchemin3 0:3a7cef09ad54 4 #include <math.h>
ebeauchemin3 0:3a7cef09ad54 5 #include <assert.h>
ebeauchemin3 0:3a7cef09ad54 6 #include "SDFileSystem.h"
ebeauchemin3 0:3a7cef09ad54 7 #include "StepperMotor_X27168.h"
ebeauchemin3 0:3a7cef09ad54 8 #include <mbed.h>
ebeauchemin3 0:3a7cef09ad54 9 #include <mpr121.h>
ebeauchemin3 0:3a7cef09ad54 10 #include "rtos.h"
ebeauchemin3 0:3a7cef09ad54 11 #include "PID.h"
ebeauchemin3 0:3a7cef09ad54 12 #include "millis/millis.h"
ebeauchemin3 0:3a7cef09ad54 13 DigitalOut myled(LED1);
ebeauchemin3 0:3a7cef09ad54 14 Serial pc(USBTX, USBRX);
ebeauchemin3 0:3a7cef09ad54 15 SDFileSystem sd(p5, p6, p7, p8, "sd");
ebeauchemin3 0:3a7cef09ad54 16
ebeauchemin3 0:3a7cef09ad54 17 float xNowPrint;
ebeauchemin3 0:3a7cef09ad54 18 float yNowPrint;
ebeauchemin3 0:3a7cef09ad54 19 float zNowPrint;
ebeauchemin3 0:3a7cef09ad54 20
ebeauchemin3 0:3a7cef09ad54 21 #define DEFAULT_Kp 5//mess with these to increase speed, was 1, .002, 20
ebeauchemin3 0:3a7cef09ad54 22 #define DEFAULT_Ki 0.01
ebeauchemin3 0:3a7cef09ad54 23 #define DEFAULT_Kd 20
ebeauchemin3 0:3a7cef09ad54 24 #define THERMISTORNOMINAL 100000 // 100k
ebeauchemin3 0:3a7cef09ad54 25 // temp. for nominal resistance (almost always 25 C)
ebeauchemin3 0:3a7cef09ad54 26 #define TEMPERATURENOMINAL 25
ebeauchemin3 0:3a7cef09ad54 27 // The beta coefficient of the thermistor (usually 3000-4000)
ebeauchemin3 0:3a7cef09ad54 28 #define BCOEFFICIENT 3950
ebeauchemin3 0:3a7cef09ad54 29 // the value of the 'other' resistor
ebeauchemin3 0:3a7cef09ad54 30 #define SERIESRESISTOR 4700
ebeauchemin3 0:3a7cef09ad54 31 #define AUTOMATIC 1
ebeauchemin3 0:3a7cef09ad54 32 #define MANUAL 0
ebeauchemin3 0:3a7cef09ad54 33 #define DIRECT 0
ebeauchemin3 0:3a7cef09ad54 34 #define REVERSE 1
ebeauchemin3 0:3a7cef09ad54 35 #define thermistor A3 // FRDM-K64F Analog input pin A3 - Adjust to your particular board
ebeauchemin3 0:3a7cef09ad54 36 #define driver PTC3 // FRDM-K64F PWM output pin PTC3 - Adjust to your particular board
ebeauchemin3 0:3a7cef09ad54 37
ebeauchemin3 0:3a7cef09ad54 38 AnalogIn Thermistor(p20);//thermistor); // Read temperature value from thermistor on A3
ebeauchemin3 0:3a7cef09ad54 39 PwmOut Driver(p25); // PWM drive FET heater on PTC3 values are 0-1.0
ebeauchemin3 0:3a7cef09ad54 40 // For 0-100%
ebeauchemin3 0:3a7cef09ad54 41
ebeauchemin3 0:3a7cef09ad54 42 float Input, Output, Setpoint, currTemp =0;
ebeauchemin3 0:3a7cef09ad54 43 PID controller(&Input, &Output, &Setpoint, DEFAULT_Kp , DEFAULT_Ki , DEFAULT_Kd , DIRECT);
ebeauchemin3 0:3a7cef09ad54 44
ebeauchemin3 0:3a7cef09ad54 45 #define RATE 1.0 // Print rate Once per second
ebeauchemin3 0:3a7cef09ad54 46
ebeauchemin3 0:3a7cef09ad54 47 float scaleX = .05;
ebeauchemin3 0:3a7cef09ad54 48 float scaleY = .1;
ebeauchemin3 0:3a7cef09ad54 49 float scaleZ = .1;
ebeauchemin3 0:3a7cef09ad54 50
ebeauchemin3 0:3a7cef09ad54 51 StepperMotor_X27168 smotor1(p23, p24, p22, p21);
ebeauchemin3 0:3a7cef09ad54 52 StepperMotor_X27168 smotor2(p9, p10, p29, p30);
ebeauchemin3 0:3a7cef09ad54 53 StepperMotor_X27168 smotor3(p14, p15, p13, p12);
ebeauchemin3 0:3a7cef09ad54 54 StepperMotor_X27168 smotor4(p17, p16, p18, p19);
ebeauchemin3 0:3a7cef09ad54 55
ebeauchemin3 0:3a7cef09ad54 56 // Setup the i2c bus on pins 9 and 10
ebeauchemin3 0:3a7cef09ad54 57 I2C i2c(p28, p27);
ebeauchemin3 0:3a7cef09ad54 58 // Setup the Mpr121:
ebeauchemin3 0:3a7cef09ad54 59 // constructor(i2c object, i2c address of the mpr121)
ebeauchemin3 0:3a7cef09ad54 60 Mpr121 mpr121(&i2c, Mpr121::ADD_VSS);
ebeauchemin3 0:3a7cef09ad54 61
ebeauchemin3 0:3a7cef09ad54 62 enum moveMotor { NONE, XPLUS, XMINUS, YPLUS, YMINUS, ZPLUS, ZMINUS };
ebeauchemin3 0:3a7cef09ad54 63 moveMotor motorMove = NONE;
ebeauchemin3 0:3a7cef09ad54 64
ebeauchemin3 0:3a7cef09ad54 65 //all motors initially stopped
ebeauchemin3 0:3a7cef09ad54 66 int motor1Go = 2;
ebeauchemin3 0:3a7cef09ad54 67 int motor2Go = 2;
ebeauchemin3 0:3a7cef09ad54 68 int motor3Go = 2;
ebeauchemin3 0:3a7cef09ad54 69 int motor4Go = 2;
ebeauchemin3 0:3a7cef09ad54 70
ebeauchemin3 0:3a7cef09ad54 71 char *strdup (const char *s)
ebeauchemin3 0:3a7cef09ad54 72 {
ebeauchemin3 0:3a7cef09ad54 73 char *d = (char*)malloc (strlen (s) + 1); // Space for length plus nul
ebeauchemin3 0:3a7cef09ad54 74 if (d == NULL) return NULL; // No memory
ebeauchemin3 0:3a7cef09ad54 75 strcpy (d,s); // Copy the characters
ebeauchemin3 0:3a7cef09ad54 76 return d; // Return the new string
ebeauchemin3 0:3a7cef09ad54 77 }
ebeauchemin3 0:3a7cef09ad54 78
ebeauchemin3 0:3a7cef09ad54 79 //split a string on a delimiter
ebeauchemin3 0:3a7cef09ad54 80 char** str_split(char* a_str, const char a_delim)
ebeauchemin3 0:3a7cef09ad54 81 {
ebeauchemin3 0:3a7cef09ad54 82 char** result = 0;
ebeauchemin3 0:3a7cef09ad54 83 size_t count = 0;
ebeauchemin3 0:3a7cef09ad54 84 char* tmp = a_str;
ebeauchemin3 0:3a7cef09ad54 85 char* last_comma = 0;
ebeauchemin3 0:3a7cef09ad54 86 char delim[2];
ebeauchemin3 0:3a7cef09ad54 87 delim[0] = a_delim;
ebeauchemin3 0:3a7cef09ad54 88 delim[1] = 0;
ebeauchemin3 0:3a7cef09ad54 89
ebeauchemin3 0:3a7cef09ad54 90 /* Count how many elements will be extracted. */
ebeauchemin3 0:3a7cef09ad54 91 while (*tmp) {
ebeauchemin3 0:3a7cef09ad54 92 if (a_delim == *tmp) {
ebeauchemin3 0:3a7cef09ad54 93 count++;
ebeauchemin3 0:3a7cef09ad54 94 last_comma = tmp;
ebeauchemin3 0:3a7cef09ad54 95 }
ebeauchemin3 0:3a7cef09ad54 96 tmp++;
ebeauchemin3 0:3a7cef09ad54 97 }
ebeauchemin3 0:3a7cef09ad54 98
ebeauchemin3 0:3a7cef09ad54 99 /* Add space for trailing token. */
ebeauchemin3 0:3a7cef09ad54 100 count += last_comma < (a_str + strlen(a_str) - 1);
ebeauchemin3 0:3a7cef09ad54 101
ebeauchemin3 0:3a7cef09ad54 102 /* Add space for terminating null string so caller
ebeauchemin3 0:3a7cef09ad54 103 knows where the list of returned strings ends. */
ebeauchemin3 0:3a7cef09ad54 104 count++;
ebeauchemin3 0:3a7cef09ad54 105
ebeauchemin3 0:3a7cef09ad54 106 result = (char**)malloc(sizeof(char*) * count);
ebeauchemin3 0:3a7cef09ad54 107
ebeauchemin3 0:3a7cef09ad54 108 if (result) {
ebeauchemin3 0:3a7cef09ad54 109 size_t idx = 0;
ebeauchemin3 0:3a7cef09ad54 110 char* token = strtok(a_str, delim);
ebeauchemin3 0:3a7cef09ad54 111
ebeauchemin3 0:3a7cef09ad54 112 while (token) {
ebeauchemin3 0:3a7cef09ad54 113 assert(idx < count);
ebeauchemin3 0:3a7cef09ad54 114 *(result + idx++) = strdup(token);
ebeauchemin3 0:3a7cef09ad54 115 token = strtok(0, delim);
ebeauchemin3 0:3a7cef09ad54 116 }
ebeauchemin3 0:3a7cef09ad54 117 assert(idx == count - 1);
ebeauchemin3 0:3a7cef09ad54 118 *(result + idx) = 0;
ebeauchemin3 0:3a7cef09ad54 119 }
ebeauchemin3 0:3a7cef09ad54 120
ebeauchemin3 0:3a7cef09ad54 121 return result;
ebeauchemin3 0:3a7cef09ad54 122 }
ebeauchemin3 0:3a7cef09ad54 123 void motorXYprint()
ebeauchemin3 0:3a7cef09ad54 124 {
ebeauchemin3 0:3a7cef09ad54 125 //smotor1.set_speed(xNowPrint/yNowPrint*200);
ebeauchemin3 0:3a7cef09ad54 126 int spinvar = 0;
ebeauchemin3 0:3a7cef09ad54 127 float xSpot = 0;
ebeauchemin3 0:3a7cef09ad54 128 float ySpot = 0;
ebeauchemin3 0:3a7cef09ad54 129 xNowPrint = floor(xNowPrint);
ebeauchemin3 0:3a7cef09ad54 130 yNowPrint = floor(yNowPrint);
ebeauchemin3 0:3a7cef09ad54 131 float product = abs(xNowPrint*yNowPrint);
ebeauchemin3 0:3a7cef09ad54 132
ebeauchemin3 0:3a7cef09ad54 133 if ((xNowPrint > 0) and (xNowPrint > 0)) {
ebeauchemin3 0:3a7cef09ad54 134 while(spinvar <= product) {
ebeauchemin3 0:3a7cef09ad54 135 spinvar ++;
ebeauchemin3 0:3a7cef09ad54 136 if (xSpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 137 smotor1.step(0);
ebeauchemin3 0:3a7cef09ad54 138 xSpot = xSpot + product/yNowPrint;
ebeauchemin3 0:3a7cef09ad54 139 }
ebeauchemin3 0:3a7cef09ad54 140 if (ySpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 141 smotor2.step(0);
ebeauchemin3 0:3a7cef09ad54 142 ySpot = ySpot + product/xNowPrint;
ebeauchemin3 0:3a7cef09ad54 143 }
ebeauchemin3 0:3a7cef09ad54 144 }
ebeauchemin3 0:3a7cef09ad54 145 } else if ((xNowPrint > 0)and(xNowPrint < 0)) {
ebeauchemin3 0:3a7cef09ad54 146 while(spinvar <= product) {
ebeauchemin3 0:3a7cef09ad54 147 spinvar ++;
ebeauchemin3 0:3a7cef09ad54 148 if (xSpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 149 smotor1.step(0);
ebeauchemin3 0:3a7cef09ad54 150 xSpot = xSpot + abs(product/yNowPrint);
ebeauchemin3 0:3a7cef09ad54 151 }
ebeauchemin3 0:3a7cef09ad54 152 if (ySpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 153 smotor2.step(1);
ebeauchemin3 0:3a7cef09ad54 154 ySpot = ySpot + abs(product/xNowPrint);
ebeauchemin3 0:3a7cef09ad54 155 }
ebeauchemin3 0:3a7cef09ad54 156 //Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 157 }
ebeauchemin3 0:3a7cef09ad54 158 } else if ((xNowPrint < 0)and(xNowPrint > 0)) {
ebeauchemin3 0:3a7cef09ad54 159 while(spinvar <= product) {
ebeauchemin3 0:3a7cef09ad54 160 spinvar ++;
ebeauchemin3 0:3a7cef09ad54 161 if (xSpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 162 smotor1.step(1);
ebeauchemin3 0:3a7cef09ad54 163 xSpot = xSpot + abs(product/yNowPrint);
ebeauchemin3 0:3a7cef09ad54 164 }
ebeauchemin3 0:3a7cef09ad54 165 if (ySpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 166 smotor2.step(0);
ebeauchemin3 0:3a7cef09ad54 167 ySpot = ySpot + abs(product/xNowPrint);
ebeauchemin3 0:3a7cef09ad54 168 }
ebeauchemin3 0:3a7cef09ad54 169 //Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 170 }
ebeauchemin3 0:3a7cef09ad54 171 } else if ((xNowPrint < 0)and(xNowPrint < 0)) {
ebeauchemin3 0:3a7cef09ad54 172 while(spinvar <= product) {
ebeauchemin3 0:3a7cef09ad54 173 spinvar ++;
ebeauchemin3 0:3a7cef09ad54 174 if (xSpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 175 smotor1.step(1);
ebeauchemin3 0:3a7cef09ad54 176 xSpot = xSpot - (product/yNowPrint);
ebeauchemin3 0:3a7cef09ad54 177 }
ebeauchemin3 0:3a7cef09ad54 178 if (ySpot < spinvar) {
ebeauchemin3 0:3a7cef09ad54 179 smotor2.step(1);
ebeauchemin3 0:3a7cef09ad54 180 ySpot = ySpot - (product/xNowPrint);
ebeauchemin3 0:3a7cef09ad54 181 }
ebeauchemin3 0:3a7cef09ad54 182 //Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 183 }
ebeauchemin3 0:3a7cef09ad54 184 }
ebeauchemin3 0:3a7cef09ad54 185 smotor1.step(2);
ebeauchemin3 0:3a7cef09ad54 186 smotor2.step(2);
ebeauchemin3 0:3a7cef09ad54 187 }
ebeauchemin3 0:3a7cef09ad54 188
ebeauchemin3 0:3a7cef09ad54 189 void motorZprint()
ebeauchemin3 0:3a7cef09ad54 190 {
ebeauchemin3 0:3a7cef09ad54 191 //smotor2.set_speed(yNowPrint/xNowPrint*200);
ebeauchemin3 0:3a7cef09ad54 192 int spinvar2 = 0;
ebeauchemin3 0:3a7cef09ad54 193 if (zNowPrint > 0) {
ebeauchemin3 0:3a7cef09ad54 194 while(spinvar2 <= zNowPrint) {
ebeauchemin3 0:3a7cef09ad54 195 spinvar2 ++;
ebeauchemin3 0:3a7cef09ad54 196 //smotor3.step(0);
ebeauchemin3 0:3a7cef09ad54 197 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 198 }
ebeauchemin3 0:3a7cef09ad54 199 } else if (zNowPrint < 0) {
ebeauchemin3 0:3a7cef09ad54 200 while(spinvar2 >= zNowPrint) {
ebeauchemin3 0:3a7cef09ad54 201 spinvar2 --;
ebeauchemin3 0:3a7cef09ad54 202 //smotor3.step(1);
ebeauchemin3 0:3a7cef09ad54 203 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 204 }
ebeauchemin3 0:3a7cef09ad54 205 }
ebeauchemin3 0:3a7cef09ad54 206 smotor3.step(2);
ebeauchemin3 0:3a7cef09ad54 207 }
ebeauchemin3 0:3a7cef09ad54 208 void print() //read from gcode file
ebeauchemin3 0:3a7cef09ad54 209 {
ebeauchemin3 0:3a7cef09ad54 210 FILE *fp = fopen("/sd/text/gCode.txt", "r");
ebeauchemin3 0:3a7cef09ad54 211 pc.printf("Hello World!\n");
ebeauchemin3 0:3a7cef09ad54 212 //FILE *stream;
ebeauchemin3 0:3a7cef09ad54 213 char line[200];
ebeauchemin3 0:3a7cef09ad54 214 char g[2]="G";
ebeauchemin3 0:3a7cef09ad54 215 char m[2]="M";
ebeauchemin3 0:3a7cef09ad54 216 char x[2]="X";
ebeauchemin3 0:3a7cef09ad54 217 char y[2]="Y";
ebeauchemin3 0:3a7cef09ad54 218 char z[2]="Z";
ebeauchemin3 0:3a7cef09ad54 219 char e[2]="E";
ebeauchemin3 0:3a7cef09ad54 220 char f[2]="F";
ebeauchemin3 0:3a7cef09ad54 221 char** tokens;
ebeauchemin3 0:3a7cef09ad54 222 char** astrktokens;
ebeauchemin3 0:3a7cef09ad54 223 float extruderX = 0;
ebeauchemin3 0:3a7cef09ad54 224 float extruderY = 0;
ebeauchemin3 0:3a7cef09ad54 225 float extruderZ = 0;
ebeauchemin3 0:3a7cef09ad54 226 float extruderE = 0;
ebeauchemin3 0:3a7cef09ad54 227 float extruderF = 0;
ebeauchemin3 0:3a7cef09ad54 228 pc.printf("here\n");
ebeauchemin3 0:3a7cef09ad54 229 //end the 4 motor threads
ebeauchemin3 0:3a7cef09ad54 230 motor1Go=3;
ebeauchemin3 0:3a7cef09ad54 231 motor2Go=3;
ebeauchemin3 0:3a7cef09ad54 232 motor3Go=3;
ebeauchemin3 0:3a7cef09ad54 233 motor4Go=3;
ebeauchemin3 0:3a7cef09ad54 234 while (fgets(line, 200, fp)) { //get a line
ebeauchemin3 0:3a7cef09ad54 235 wait(.5);
ebeauchemin3 0:3a7cef09ad54 236 char firstLet[sizeof(line)];
ebeauchemin3 0:3a7cef09ad54 237 strncpy(firstLet, line,1);
ebeauchemin3 0:3a7cef09ad54 238 firstLet[1] = 0;
ebeauchemin3 0:3a7cef09ad54 239 if (strcmp(firstLet,g) * strcmp(firstLet,m) == 0) {
ebeauchemin3 0:3a7cef09ad54 240 astrktokens = str_split(line, ';');
ebeauchemin3 0:3a7cef09ad54 241 char* tempdata = *(astrktokens);
ebeauchemin3 0:3a7cef09ad54 242 //pc.printf("Temp data: %s", tempdata);
ebeauchemin3 0:3a7cef09ad54 243 char fullData[100];
ebeauchemin3 0:3a7cef09ad54 244 //char singleVariable[100];
ebeauchemin3 0:3a7cef09ad54 245 strcpy(fullData, tempdata);
ebeauchemin3 0:3a7cef09ad54 246 tokens = str_split(tempdata, ' ');
ebeauchemin3 0:3a7cef09ad54 247 if (tokens) {
ebeauchemin3 0:3a7cef09ad54 248
ebeauchemin3 0:3a7cef09ad54 249 int i = 0;
ebeauchemin3 0:3a7cef09ad54 250 //ssize_t
ebeauchemin3 0:3a7cef09ad54 251 char* data = *(tokens + i);
ebeauchemin3 0:3a7cef09ad54 252 pc.printf("Data: %s ", data);
ebeauchemin3 0:3a7cef09ad54 253 wait(.5);
ebeauchemin3 0:3a7cef09ad54 254 if (strcmp(data,"G0") * strcmp(data,"G1") == 0) {
ebeauchemin3 0:3a7cef09ad54 255 //Move: X,Y,Z-postions to move to, E-extrude amount, F-feedrate
ebeauchemin3 0:3a7cef09ad54 256 pc.printf("Move to: ");
ebeauchemin3 0:3a7cef09ad54 257 i = 1;
ebeauchemin3 0:3a7cef09ad54 258 float newX = 0;
ebeauchemin3 0:3a7cef09ad54 259 float newY = 0;
ebeauchemin3 0:3a7cef09ad54 260 float newZ = 0;
ebeauchemin3 0:3a7cef09ad54 261 float newE = 0;
ebeauchemin3 0:3a7cef09ad54 262 float newF = 0;
ebeauchemin3 0:3a7cef09ad54 263 while(strcmp(*(tokens + i),"\0") != 0) {
ebeauchemin3 0:3a7cef09ad54 264 data = *(tokens + i);
ebeauchemin3 0:3a7cef09ad54 265 //strcpy(singleVariable, data);
ebeauchemin3 0:3a7cef09ad54 266 //pc.printf("Deep Data: %s ", data);
ebeauchemin3 0:3a7cef09ad54 267 char firstLet[sizeof(data)];
ebeauchemin3 0:3a7cef09ad54 268 strncpy(firstLet, data,1);
ebeauchemin3 0:3a7cef09ad54 269 firstLet[1] = 0;
ebeauchemin3 0:3a7cef09ad54 270 if (strcmp(firstLet,x) == 0) {
ebeauchemin3 0:3a7cef09ad54 271 memmove(data, data+1, strlen(data));
ebeauchemin3 0:3a7cef09ad54 272 newX = strtof(data, NULL);
ebeauchemin3 0:3a7cef09ad54 273 pc.printf("X: %f", newX);
ebeauchemin3 0:3a7cef09ad54 274 if (extruderX == 0) {
ebeauchemin3 0:3a7cef09ad54 275 extruderX = newX;
ebeauchemin3 0:3a7cef09ad54 276 }
ebeauchemin3 0:3a7cef09ad54 277 } else if (strcmp(firstLet,y) == 0) {
ebeauchemin3 0:3a7cef09ad54 278 memmove(data, data+1, strlen(data));
ebeauchemin3 0:3a7cef09ad54 279 newY = strtof(data, NULL);
ebeauchemin3 0:3a7cef09ad54 280 pc.printf("Y: %f", newY);
ebeauchemin3 0:3a7cef09ad54 281 if (extruderY == 0) {
ebeauchemin3 0:3a7cef09ad54 282 extruderY = newY;
ebeauchemin3 0:3a7cef09ad54 283 }
ebeauchemin3 0:3a7cef09ad54 284 } else if (strcmp(firstLet,z) == 0) {
ebeauchemin3 0:3a7cef09ad54 285 memmove(data, data+1, strlen(data));
ebeauchemin3 0:3a7cef09ad54 286 newZ = strtof(data, NULL);
ebeauchemin3 0:3a7cef09ad54 287 pc.printf("Z: %f", newZ);
ebeauchemin3 0:3a7cef09ad54 288 if (extruderZ == 0) {
ebeauchemin3 0:3a7cef09ad54 289 extruderZ = newZ;
ebeauchemin3 0:3a7cef09ad54 290 }
ebeauchemin3 0:3a7cef09ad54 291 } else if (strcmp(firstLet,e) == 0) {
ebeauchemin3 0:3a7cef09ad54 292 memmove(data, data+1, strlen(data));
ebeauchemin3 0:3a7cef09ad54 293 newE = strtof(data, NULL);
ebeauchemin3 0:3a7cef09ad54 294 pc.printf("E: %f", newE);
ebeauchemin3 0:3a7cef09ad54 295 } else if (strcmp(firstLet,f) == 0) {
ebeauchemin3 0:3a7cef09ad54 296 memmove(data, data+1, strlen(data));
ebeauchemin3 0:3a7cef09ad54 297 newF = strtof(data, NULL);
ebeauchemin3 0:3a7cef09ad54 298 pc.printf("F: %f", newF);
ebeauchemin3 0:3a7cef09ad54 299 }
ebeauchemin3 0:3a7cef09ad54 300 newX = newX-extruderX;
ebeauchemin3 0:3a7cef09ad54 301 newY = newY-extruderY;
ebeauchemin3 0:3a7cef09ad54 302 newZ = newZ-extruderZ;
ebeauchemin3 0:3a7cef09ad54 303 xNowPrint = newX*scaleX;
ebeauchemin3 0:3a7cef09ad54 304 yNowPrint = newY*scaleY;
ebeauchemin3 0:3a7cef09ad54 305 zNowPrint = newZ*scaleZ;
ebeauchemin3 0:3a7cef09ad54 306 if ((xNowPrint != 0) and (yNowPrint != 0)) {
ebeauchemin3 0:3a7cef09ad54 307 pc.printf("xNow: %f", xNowPrint);
ebeauchemin3 0:3a7cef09ad54 308 pc.printf("yNow: %f", yNowPrint);
ebeauchemin3 0:3a7cef09ad54 309 motorXYprint();
ebeauchemin3 0:3a7cef09ad54 310 }
ebeauchemin3 0:3a7cef09ad54 311 if (zNowPrint != 0) {
ebeauchemin3 0:3a7cef09ad54 312 pc.printf("yNow: %f", yNowPrint);
ebeauchemin3 0:3a7cef09ad54 313 motorZprint();
ebeauchemin3 0:3a7cef09ad54 314 }
ebeauchemin3 0:3a7cef09ad54 315
ebeauchemin3 0:3a7cef09ad54 316
ebeauchemin3 0:3a7cef09ad54 317 i = i+1;
ebeauchemin3 0:3a7cef09ad54 318 }
ebeauchemin3 0:3a7cef09ad54 319 }
ebeauchemin3 0:3a7cef09ad54 320 }
ebeauchemin3 0:3a7cef09ad54 321 }
ebeauchemin3 0:3a7cef09ad54 322 myled = 1;
ebeauchemin3 0:3a7cef09ad54 323 wait(0.2);
ebeauchemin3 0:3a7cef09ad54 324 myled = 0;
ebeauchemin3 0:3a7cef09ad54 325 wait(0.2);
ebeauchemin3 0:3a7cef09ad54 326 }
ebeauchemin3 0:3a7cef09ad54 327 fclose(fp);
ebeauchemin3 0:3a7cef09ad54 328 return;
ebeauchemin3 0:3a7cef09ad54 329 }
ebeauchemin3 0:3a7cef09ad54 330
ebeauchemin3 0:3a7cef09ad54 331
ebeauchemin3 0:3a7cef09ad54 332 void motor1Thread(void const *args)
ebeauchemin3 0:3a7cef09ad54 333 {
ebeauchemin3 0:3a7cef09ad54 334 smotor1.set_speed(80);
ebeauchemin3 0:3a7cef09ad54 335 int spinvar = 0;
ebeauchemin3 0:3a7cef09ad54 336 while(motor1Go<3) { //3 stops the thread
ebeauchemin3 0:3a7cef09ad54 337 while(motor1Go != 2) {
ebeauchemin3 0:3a7cef09ad54 338 spinvar = 0;
ebeauchemin3 0:3a7cef09ad54 339 if (motor1Go == 0) {
ebeauchemin3 0:3a7cef09ad54 340 smotor1.step(0);
ebeauchemin3 0:3a7cef09ad54 341
ebeauchemin3 0:3a7cef09ad54 342 } else if(motor1Go == 1) {
ebeauchemin3 0:3a7cef09ad54 343 smotor1.step(1);
ebeauchemin3 0:3a7cef09ad54 344 }
ebeauchemin3 0:3a7cef09ad54 345 }
ebeauchemin3 0:3a7cef09ad54 346 smotor1.step(2);
ebeauchemin3 0:3a7cef09ad54 347 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 348 }
ebeauchemin3 0:3a7cef09ad54 349 }
ebeauchemin3 0:3a7cef09ad54 350
ebeauchemin3 0:3a7cef09ad54 351 void motor2Thread(void const *args)
ebeauchemin3 0:3a7cef09ad54 352 {
ebeauchemin3 0:3a7cef09ad54 353 smotor2.set_speed(80);
ebeauchemin3 0:3a7cef09ad54 354 int spinvar2 = 0;
ebeauchemin3 0:3a7cef09ad54 355 while(motor2Go<3) { //3 stops the thread
ebeauchemin3 0:3a7cef09ad54 356 while(motor2Go != 2) {
ebeauchemin3 0:3a7cef09ad54 357 spinvar2 = 0;
ebeauchemin3 0:3a7cef09ad54 358 if (motor2Go == 0) {
ebeauchemin3 0:3a7cef09ad54 359 smotor2.step(0);
ebeauchemin3 0:3a7cef09ad54 360 } else if(motor2Go == 1) {
ebeauchemin3 0:3a7cef09ad54 361 smotor2.step(1);
ebeauchemin3 0:3a7cef09ad54 362 }
ebeauchemin3 0:3a7cef09ad54 363 }
ebeauchemin3 0:3a7cef09ad54 364 smotor2.step(2);
ebeauchemin3 0:3a7cef09ad54 365 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 366 }
ebeauchemin3 0:3a7cef09ad54 367 }
ebeauchemin3 0:3a7cef09ad54 368 void motor3Thread(void const *args)
ebeauchemin3 0:3a7cef09ad54 369 {
ebeauchemin3 0:3a7cef09ad54 370 smotor3.set_speed(80);
ebeauchemin3 0:3a7cef09ad54 371 int spinvar3 = 0;
ebeauchemin3 0:3a7cef09ad54 372 while(motor3Go<3) { //3 stops the thread
ebeauchemin3 0:3a7cef09ad54 373 while(motor3Go != 2) {
ebeauchemin3 0:3a7cef09ad54 374 spinvar3 = 0;
ebeauchemin3 0:3a7cef09ad54 375 if (motor3Go == 0) {
ebeauchemin3 0:3a7cef09ad54 376 smotor3.step(0);
ebeauchemin3 0:3a7cef09ad54 377
ebeauchemin3 0:3a7cef09ad54 378 } else if(motor3Go == 1) {
ebeauchemin3 0:3a7cef09ad54 379 smotor3.step(1);
ebeauchemin3 0:3a7cef09ad54 380 }
ebeauchemin3 0:3a7cef09ad54 381 }
ebeauchemin3 0:3a7cef09ad54 382 smotor3.step(2);
ebeauchemin3 0:3a7cef09ad54 383 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 384 }
ebeauchemin3 0:3a7cef09ad54 385 }
ebeauchemin3 0:3a7cef09ad54 386 void motor4Thread(void const *args)
ebeauchemin3 0:3a7cef09ad54 387 {
ebeauchemin3 0:3a7cef09ad54 388 smotor4.set_speed(80); //unnecessary but oh well
ebeauchemin3 0:3a7cef09ad54 389 int spinvar4 = 0;
ebeauchemin3 0:3a7cef09ad54 390 while(motor4Go<3) {
ebeauchemin3 0:3a7cef09ad54 391 while(motor4Go != 2) {
ebeauchemin3 0:3a7cef09ad54 392 spinvar4 = 0;
ebeauchemin3 0:3a7cef09ad54 393 if (motor4Go == 0) {
ebeauchemin3 0:3a7cef09ad54 394 smotor4.step(0);
ebeauchemin3 0:3a7cef09ad54 395 } else if(motor4Go == 1) {
ebeauchemin3 0:3a7cef09ad54 396 smotor4.step(1);
ebeauchemin3 0:3a7cef09ad54 397 }
ebeauchemin3 0:3a7cef09ad54 398 }
ebeauchemin3 0:3a7cef09ad54 399 smotor4.step(2);
ebeauchemin3 0:3a7cef09ad54 400 Thread::wait(50.0);
ebeauchemin3 0:3a7cef09ad54 401 }
ebeauchemin3 0:3a7cef09ad54 402 }
ebeauchemin3 0:3a7cef09ad54 403
ebeauchemin3 0:3a7cef09ad54 404 void getTemperature(void const *args)
ebeauchemin3 0:3a7cef09ad54 405 {
ebeauchemin3 0:3a7cef09ad54 406 // This routine calculates the temperature
ebeauchemin3 0:3a7cef09ad54 407 // using the Steinhart-Hart equation for thermistors
ebeauchemin3 0:3a7cef09ad54 408 // https://en.wikipedia.org/wiki/Steinhart%E2%80%93Hart_equation
ebeauchemin3 0:3a7cef09ad54 409 float temperature, resistance;
ebeauchemin3 0:3a7cef09ad54 410 float steinhart;
ebeauchemin3 0:3a7cef09ad54 411 int a;
ebeauchemin3 0:3a7cef09ad54 412 Setpoint = 200; // Set target temperature in degrees Celcius.
ebeauchemin3 0:3a7cef09ad54 413 controller.SetMode(AUTOMATIC); // Turn PID controller on.
ebeauchemin3 0:3a7cef09ad54 414 while(1) {
ebeauchemin3 0:3a7cef09ad54 415 controller.Compute(); // Process PID loop.
ebeauchemin3 0:3a7cef09ad54 416 //Driver = Output/1000; // Sent PWM value scaled 0 - 1.0 as mbed requires ***originally divided by 1000
ebeauchemin3 0:3a7cef09ad54 417 if (Input < 270) {
ebeauchemin3 0:3a7cef09ad54 418 Driver = .9;
ebeauchemin3 0:3a7cef09ad54 419 } else {
ebeauchemin3 0:3a7cef09ad54 420 Driver = 0;
ebeauchemin3 0:3a7cef09ad54 421 }
ebeauchemin3 0:3a7cef09ad54 422 a = Thermistor.read_u16(); // Read 16bit Analog value
ebeauchemin3 0:3a7cef09ad54 423 // pc.printf("Raw Analog Value for Thermistor = %d\r\n",a);
ebeauchemin3 0:3a7cef09ad54 424 /* Calculate the resistance of the thermistor from analog votage read. */
ebeauchemin3 0:3a7cef09ad54 425 resistance = (float) SERIESRESISTOR / ((65536.0 / a) - 1);
ebeauchemin3 0:3a7cef09ad54 426 // pc.printf("Resistance for Thermistor = %f\r\n",resistance);
ebeauchemin3 0:3a7cef09ad54 427
ebeauchemin3 0:3a7cef09ad54 428 steinhart = resistance / THERMISTORNOMINAL; // (R/Ro)
ebeauchemin3 0:3a7cef09ad54 429 steinhart = log(steinhart); // ln(R/Ro)
ebeauchemin3 0:3a7cef09ad54 430 steinhart /= BCOEFFICIENT; // 1/B * ln(R/Ro)
ebeauchemin3 0:3a7cef09ad54 431 steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/To)
ebeauchemin3 0:3a7cef09ad54 432 steinhart = 1.0 / steinhart; // Invert
ebeauchemin3 0:3a7cef09ad54 433 temperature = steinhart - 273.15; // convert to C
ebeauchemin3 0:3a7cef09ad54 434 // pc.printf("Extruder Temperature is %f\r\n", temperature);
ebeauchemin3 0:3a7cef09ad54 435 Input = temperature;
ebeauchemin3 0:3a7cef09ad54 436 pc.printf("Input Output Setpoint Kp Ki Kd time\r\n");
ebeauchemin3 0:3a7cef09ad54 437 pc.printf("%f, %f, %f, %f, %f, %f, %d \r\n", temperature, Output, Setpoint, controller.GetKp() , controller.GetKi() , controller.GetKd() , millis() );
ebeauchemin3 0:3a7cef09ad54 438 Thread::wait(1000);
ebeauchemin3 0:3a7cef09ad54 439 }
ebeauchemin3 0:3a7cef09ad54 440
ebeauchemin3 0:3a7cef09ad54 441 }
ebeauchemin3 0:3a7cef09ad54 442 int main()
ebeauchemin3 0:3a7cef09ad54 443 {
ebeauchemin3 0:3a7cef09ad54 444
ebeauchemin3 0:3a7cef09ad54 445 int x = 0;
ebeauchemin3 0:3a7cef09ad54 446 int key_code=0;
ebeauchemin3 0:3a7cef09ad54 447 int i=0;
ebeauchemin3 0:3a7cef09ad54 448 int value;
ebeauchemin3 0:3a7cef09ad54 449 startMillis(); // Initialize timer.
ebeauchemin3 0:3a7cef09ad54 450 //terminal prints status data
ebeauchemin3 0:3a7cef09ad54 451 pc.baud(115200);
ebeauchemin3 0:3a7cef09ad54 452 pc.printf("\r\nThermistor PID Test - Build " __DATE__ " " __TIME__ "\r\n");
ebeauchemin3 0:3a7cef09ad54 453 //motors 1-3 are x,y,or z
ebeauchemin3 0:3a7cef09ad54 454 Thread thread1(motor1Thread);
ebeauchemin3 0:3a7cef09ad54 455 Thread thread2(motor2Thread);
ebeauchemin3 0:3a7cef09ad54 456 Thread thread3(motor3Thread);
ebeauchemin3 0:3a7cef09ad54 457 //motor 4 is the feeder motor
ebeauchemin3 0:3a7cef09ad54 458 Thread thread4(motor4Thread);
ebeauchemin3 0:3a7cef09ad54 459 while(1) {
ebeauchemin3 0:3a7cef09ad54 460 //use keypad for controlling motors and temperature while debugging
ebeauchemin3 0:3a7cef09ad54 461 value=mpr121.read(0x00);
ebeauchemin3 0:3a7cef09ad54 462 value +=mpr121.read(0x01)<<8;
ebeauchemin3 0:3a7cef09ad54 463 if((value>>0)&0x01) { //zero button pressed
ebeauchemin3 0:3a7cef09ad54 464 myled = 1;
ebeauchemin3 0:3a7cef09ad54 465 motor1Go = 0; //x motor move back
ebeauchemin3 0:3a7cef09ad54 466 } else if((value>>1)&0x01) { //one button pressed
ebeauchemin3 0:3a7cef09ad54 467 myled = 0;
ebeauchemin3 0:3a7cef09ad54 468 motor1Go = 1; //x motor move forwards
ebeauchemin3 0:3a7cef09ad54 469 } else {
ebeauchemin3 0:3a7cef09ad54 470 motor1Go = 2; //x motor stop
ebeauchemin3 0:3a7cef09ad54 471 }
ebeauchemin3 0:3a7cef09ad54 472 if(((value>>2)&0x01)==1) { //two button pressed
ebeauchemin3 0:3a7cef09ad54 473 myled = 1;
ebeauchemin3 0:3a7cef09ad54 474 motor2Go = 0; //y motor move backward
ebeauchemin3 0:3a7cef09ad54 475 } else if(((value>>3)&0x01)==1) { //three button pressed
ebeauchemin3 0:3a7cef09ad54 476 myled = 0;
ebeauchemin3 0:3a7cef09ad54 477 motor2Go = 1; //y motor forward
ebeauchemin3 0:3a7cef09ad54 478 } else {
ebeauchemin3 0:3a7cef09ad54 479 motor2Go = 2; //else stop y motor
ebeauchemin3 0:3a7cef09ad54 480 }//end if
ebeauchemin3 0:3a7cef09ad54 481 if(((value>>4)&0x01)==1) { //four button pressed
ebeauchemin3 0:3a7cef09ad54 482 myled = 1;
ebeauchemin3 0:3a7cef09ad54 483 motor3Go = 0; //z motor back
ebeauchemin3 0:3a7cef09ad54 484 } else if(((value>>5)&0x01)==1) {
ebeauchemin3 0:3a7cef09ad54 485 myled = 0;
ebeauchemin3 0:3a7cef09ad54 486 motor3Go = 1; //z motor forward
ebeauchemin3 0:3a7cef09ad54 487 } else {
ebeauchemin3 0:3a7cef09ad54 488 motor3Go = 2; //else stop z motor
ebeauchemin3 0:3a7cef09ad54 489 }//end if
ebeauchemin3 0:3a7cef09ad54 490 if((value>>6)&0x01) { //6 button pressed
ebeauchemin3 0:3a7cef09ad54 491 myled = 1;
ebeauchemin3 0:3a7cef09ad54 492 motor4Go = 0;
ebeauchemin3 0:3a7cef09ad54 493 } else if((value>>7)&0x01) { //7 button pressed
ebeauchemin3 0:3a7cef09ad54 494 myled = 0;
ebeauchemin3 0:3a7cef09ad54 495 motor4Go = 1; //motor 4 forwards
ebeauchemin3 0:3a7cef09ad54 496 } else {
ebeauchemin3 0:3a7cef09ad54 497 motor4Go = 2; //stop motor 4
ebeauchemin3 0:3a7cef09ad54 498 }//end if
ebeauchemin3 0:3a7cef09ad54 499 if(((value>>8)&0x01)==1) { //8 button pressed
ebeauchemin3 0:3a7cef09ad54 500 myled = !myled;
ebeauchemin3 0:3a7cef09ad54 501 Setpoint +=1; //increase set temp (C) by 1
ebeauchemin3 0:3a7cef09ad54 502 } else if(((value>>9)&0x01)==1) { //button 9 pressed
ebeauchemin3 0:3a7cef09ad54 503 myled = !myled;
ebeauchemin3 0:3a7cef09ad54 504 Setpoint -= 1; //decrease set point by 1
ebeauchemin3 0:3a7cef09ad54 505 }
ebeauchemin3 0:3a7cef09ad54 506 if((value>>10)&0x01) { //10 button pressed
ebeauchemin3 0:3a7cef09ad54 507 myled = 1;
ebeauchemin3 0:3a7cef09ad54 508 print();
ebeauchemin3 0:3a7cef09ad54 509 }
ebeauchemin3 0:3a7cef09ad54 510 Thread::wait(100);
ebeauchemin3 0:3a7cef09ad54 511 }//end while(1)
ebeauchemin3 0:3a7cef09ad54 512
ebeauchemin3 0:3a7cef09ad54 513 }
ebeauchemin3 0:3a7cef09ad54 514
ebeauchemin3 0:3a7cef09ad54 515