Skip to content
Snippets Groups Projects
Commit 01f9ab09 authored by Vilde Rieker's avatar Vilde Rieker
Browse files

clean-up and GUI implementation

parent d2ab6994
Branches
No related tags found
1 merge request!20Doublefilter
Expand all Hide whitespace changes
Inline Side-by-side
4DrobotServer/4DrobotServer_config.h
+ 2
− 2
View file @ 01f9ab09
......@@ -87,8 +87,8 @@ const int grabber_stepWait = 50; //[ms]
// Yeah, a struct would be cleaner, then we could shared the code...
// {Filter1, filter2}
const size_t filter_numfilters = 2;
const int filter_servo_pin[] = {8,??};
const size_t filter_numFilters = 2;
const int filter_servo_pin[] = {8,13};
const int filter_in[] = {5,165}; //[deg]
const int filter_out[] = {165,5}; //[deg]
const int filter_min = 0; //[deg]
......
4DrobotServer/4DrobotServer_globalVars.h
+ 4
− 4
View file @ 01f9ab09
......@@ -52,16 +52,16 @@ int grabber_pos = grabber_closed; //Assumed initial position
// Camera filter servo
#ifdef USE_STDSERVOLIB
Servo filter_servo[filter_numfilters];
Servo filter_servo[filter_numFilters];
#else
Adafruit_TiCoServo filter_servo[filter_numfilters];
Adafruit_TiCoServo filter_servo[filter_numFilters];
#endif
//{FILTER1, FILTER2}
bool filter_go[] = {false, false};
int filter_goto[] = {0,0};
int filter_pos[] = {filter_out,filter_out}; //Assumed initial position
int filter_goto[] = {0,180};
int filter_pos[] = {filter_out[0],filter_out[1]}; //Assumed initial position
......
4DrobotServer/NetworkIO.ino
+ 132
− 106
View file @ 01f9ab09
......@@ -594,113 +594,139 @@ bool parse_line(char* line_buff) {
}
else if (len >= 6 and strncmp(line_buff, "FILTER", 6)==0) {
if (len >= 13 and strncmp(line_buff+6, " STATUS", 7)==0) { //'FILTER 1 STATUS'
char buff[5];
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER CONFIG: IN = "));
snprintf(buff, sizeof(buff), "%03d", filter1_in);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" OUT = "));
snprintf(buff, sizeof(buff), "%03d", filter1_out);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" LIMITS: MIN = "));
snprintf(buff, sizeof(buff), "%03d", filter1_min);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" MAX = "));
snprintf(buff, sizeof(buff), "%03d\n", filter1_max);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER POS = "));
snprintf(buff,sizeof(buff), "%03d\n", filter1_pos);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< EMERGENCY STOP = "));
if(digitalRead(emergency_stop_pin) == emergency_isEmergency) {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("TRUE\n"));
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("FALSE\n"));
}
}
else if (len >= 9 and strncmp(line_buff+6, " IN", 3)==0) { //'FILTER 1 IN'
if(filter1_go) {
// This should not be possible!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER ALREADY SET"));
return true;
}
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter1_in);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
filter1_goto = filter1_in;
filter1_go = true;
return false;
}
else if (len >= 10 and strncmp(line_buff+6, " OUT", 4)==0) { //'FILTER 1 OUT'
if(filter1_go) {
// This should not be possible!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER ALREADY SET"));
return true;
}
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter1_out);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
filter1_goto = filter1_out;
filter1_go = true;
return false;
}
else if (len >= 10 and strncmp(line_buff+6, " POS", 4) == 0) { //'FILTER 1 POS'
int scanStatus = 0;
if (len < 12) {
//no new POS integer given?
scanStatus = 0;
}
else if (len > 14) {
//More than three digits given or extra space and >2 digits?
scanStatus = 0;
}
else if (line_buff[10] != ' ') {
//No separating space?
scanStatus = 0;
}
else {
//Passed sanity checks -> scan!
scanStatus = sscanf(line_buff+11," %d",&filter1_goto);
}
if (scanStatus == 1){
//Got one thing from sscanf()!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter1_goto);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
if (filter1_goto >= filter1_min && filter1_goto <= filter1_max) {
filter1_go=true;
return false;
for (int filter=0; filter<filter_numFilters; filter++){
if (len >= 8 and strncmp(line_buff+6, " "+filter+1,2)==0){ // need "filter selector in GUI that passes a 1 or 2"?
if (len >= 15 and strncmp(line_buff+8, " STATUS", 7)==0) { //FILTER X STATUS
char buff[5];
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER "))
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< CONFIG: IN = "));
snprintf(buff, sizeof(buff), "%03d", filter_in[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" OUT= "));
snprintf(buff, sizeof(buff), "%03d", filter_out[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" LIMITS: MIN = "));
snprintf(buff, sizeof(buff), "%03d", filter_min);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F(" MAX = "));
snprintf(buff, sizeof(buff), "%03d\n", filter1_max);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< POS = "));
snprintf(buff,sizeof(buff), "%03d\n", filter_pos[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< EMERGENCY STOP = "));
if(digitalRead(emergency_stop_pin) == emergency_isEmergency) {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("TRUE\n"));
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("FALSE\n"));
}
}
else if (len >= 11 and strncmp(line_buff+8, " IN", 3)==0) { //'FILTER X IN'
if(filter_go[filter]) {
// This should not be possible!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ALREADY SET"));
return true;
}
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter_in[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
filter_goto[filter] = filter1_in[filter];
filter_go[filter] = true;
return false;
}//IN
else if (len >= 12 and strncmp(line_buff+8, " OUT", 4)==0) { //'FILTER X OUT'
for
if(filter_go[filter]) {
// This should not be possible!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ALREADY SET")); return true;
}
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter_out[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
filter_goto[filter] = filter_out[filter];
filter_go[filter] = true;
return false;
}//OUT
else if (len >= 12 and strncmp(line_buff+8, " POS", 4) == 0) { //'FILTER X POS'
int scanStatus = 0;
if (len < 14) {
//no new POS integer given?
scanStatus = 0;
}
else if (len > 16) {
//More than three digits given or extra space and >2 digits?
scanStatus = 0;
}
else if (line_buff[12] != ' ') {
//No separating space?
scanStatus = 0;
}
else {
//Passed sanity checks -> scan!
scanStatus = sscanf(line_buff+13," %d",&filter_goto[filter]);
}
if (scanStatus == 1){
//Got one thing from sscanf()!
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GOTO "));
char buff[5];
snprintf(buff,sizeof(buff), "%03d\n", filter_goto[filter]);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
if (filter_goto[filter] >= filter_min && filter_goto[filter] <= filter_max) {
filter_go[filter]=true;
return false;
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< POS OUTSIDE LEGAL RANGE\n"));
filter_goto[filter] = filter_pos[filter];
}
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER "));
snprintf(buff, sizeof(buff), "%03d", filter+1);
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< POS PARSE FAILURE\n"));
}
}//POS
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER POS OUTSIDE LEGAL RANGE\n"));
filter1_goto = filter1_pos;
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER COMMAND NOT RECOGNIZED\n"));
}
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER POS PARSE FAILURE\n"));
}
}
else {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR FILTER COMMAND NOT RECOGNIZED\n"));
}
}
}//if filter #
}//filter for loop
}//FILTER
else if (len >= 7 and strncmp(line_buff, "STEPPER", 7)==0) {
......@@ -733,7 +759,7 @@ bool parse_line(char* line_buff) {
bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
}
}
}// STATUS
else if (len >= 11 and strncmp(line_buff+7, " POS", 4)==0) { // 'STEPPER POS <axis> <steps>'
stepper_go_axis = parse_line_stepperAxis(line_buff, 11, len);
if (stepper_go_axis == SIZE_MAX) {
......@@ -778,7 +804,7 @@ bool parse_line(char* line_buff) {
return true;
}
}
}//POS
else if (len >= 16 and strncmp(line_buff+7, " ZEROSEEK", 9)==0) { // 'STEPPER ZEROSEEK <axis>'
stepper_go_axis = parse_line_stepperAxis(line_buff, 16, len);
......
4DrobotServer/ServoControl.ino
+ 3
− 5
View file @ 01f9ab09
......@@ -21,7 +21,7 @@ void setup_servo() {
}
//Sanity check filter config
for (int i=0; i< filter_numfilters; i++){
for (int i=0; i< filter_numFilters; i++){
if (not ((filter_in[i] >= filter_min && filter_in[i] <= filter_max))) {
Serial.print(F("Filter config error; filter_in outside of legal range\n"));
while(true) {
......@@ -41,7 +41,7 @@ void setup_servo() {
grabber_servo.attach(grabber_servo_pin);
grabber_servo.write(grabber_pos);
for (int i=0; i< filter_numfilters; i++){
for (int i=0; i< filter_numFilters; i++){
filter_servo[i].attach(filter_servo_pin[i], filter_minpulsewidth, filter_maxpulsewidth);
filter_servo[i].write(filter_pos[i]);
......@@ -61,7 +61,7 @@ void servo_control() {
grabber_control();
}
for (int i=0; i< filter_numfilters; i++){
for (int i=0; i< filter_numFilters; i++){
if (filter_go[i]) {
filter_control(i);
}
......@@ -144,5 +144,3 @@ void filter_control(int filter) {
output_buff_flush();
}
MatLab/Robot_GUI_exported.m
+ 113
− 342
View file @ 01f9ab09
This diff is collapsed.
MatLab/robotConnector.m
+ 45
− 39
View file @ 01f9ab09
......@@ -69,15 +69,16 @@ classdef robotConnector < handle
grabberGotoTarget = NaN; % Where is the grabber going to?
%Filter status
filterPos = NaN; % Current positon of the filter [deg]
filterID = [0,1]
filterPos = [NaN,NaN]; % Current positon of the filter [deg]
filterInPos = NaN; % Position of the filter when in
filterOutPos = NaN; % Position of the filter when out
filterInPos = [NaN,NaN]; % Position of the filter when in
filterOutPos = [NaN,NaN]; % Position of the filter when out
filterMinPos = NaN; % Smallest allowed position setting for filter servo
filterMaxPos = NaN; % Largest allowed position setting for filter servo
filterIsMoving = false; % Is the filter currently in motion?
filterGotoTarget = NaN; % Where is the filter trying to get to?
filterIsMoving = [false, false]; % Is the filter currently in motion?
filterGotoTarget = [NaN,NaN]; % Where is the filter trying to get to?
%Temperatures [degC]
temps = [];
......@@ -241,8 +242,8 @@ classdef robotConnector < handle
ok = false;
return
end
obj.TCPconn.write(['FILTER STATUS',newline])
obj.TCPconn.write(['FILTER '+obj.filterID+' STATUS',newline])
ok=true;
return
......@@ -334,7 +335,7 @@ classdef robotConnector < handle
if ~(pos >= obj.filterMinPos && pos <= obj.filterMaxPos)
error('Requested filter pos out of range')
end
obj.TCPconn.write(['FILTER POS ', num2str(round(pos)), newline]);
obj.TCPconn.write(['FILTER '+obj.filterID+' POS ', num2str(round(pos)), newline]);
ok=true;
return
......@@ -347,7 +348,7 @@ classdef robotConnector < handle
return;
end
obj.TCPconn.write(['FILTER IN',newline]);
obj.TCPconn.write(['FILTER '+obj.filterID+' IN',newline]);
ok=true;
return
......@@ -360,7 +361,7 @@ classdef robotConnector < handle
return;
end
obj.TCPconn.write(['FILTER OUT',newline]);
obj.TCPconn.write(['FILTER '+obj.filterID+' OUT',newline]);
ok=true;
return
......@@ -670,10 +671,13 @@ classdef robotConnector < handle
%Set to true if we are expecting output to be parsed
if obj.lastCommand == "STEPPER STATUS" || ...
obj.lastCommand == "GRABBER STATUS" || ...
obj.lastCommand == "FILTER STATUS" || ...
obj.lastCommand == "TEMP" || ...
obj.lastCommand == "GRABBER OPEN" || obj.lastCommand == "GRABBER CLOSE" || startsWith(obj.lastCommand,'GRABBER POS ') || ...
obj.lastCommand == "FILTER IN" || obj.lastCommand == "FILTER OUT" || startsWith(obj.lastCommand,'FILTER POS ') || ...
obj.lastCommand == "FILTER "+obj.filterID+" STATUS" || ...
obj.lastCommand == "FILTER "+obj.filterID+" IN" || obj.lastCommand == "FILTER "+tostring(i+1)+" OUT" || startsWith(obj.lastCommand,'FILTER "+tostring(i+1)+" POS ') || ...
startsWith(obj.lastCommand, 'STEPPER INTERLOCK ') || ...
startsWith(obj.lastCommand, 'STEPPER RESET ') || ...
startsWith(obj.lastCommand, 'STEPPER ZEROSEEK ') || startsWith(obj.lastCommand, 'STEPPER POS ')
......@@ -762,29 +766,7 @@ classdef robotConnector < handle
end
end
elseif obj.lastCommand == "FILTER STATUS"
lineSplit = split(line);
if obj.linesWithLastCommand == 1
obj.filterInPos = str2double(lineSplit(6));
obj.filterOutPos = str2double(lineSplit(9));
obj.filterMinPos = str2double(lineSplit(13));
obj.filterMaxPos = str2double(lineSplit(16));
elseif obj.linesWithLastCommand == 2
obj.filterPos = str2double(lineSplit(5));
elseif obj.linesWithLastCommand == 3
if lineSplit(5) == 'TRUE'
obj.isEmergencyStopped = true;
elseif lineSplit(5) == 'FALSE'
obj.isEmergencyStopped = false;
else
error(strcat("Unknown bool '", string(lineSplit(5)), "', expected TRUE or FALSE"))
end
obj.commandIsRunning = false;
if obj.callbackEnabled
obj.callbackFunction(obj,false,false,false,true);
end
end
elseif obj.lastCommand == "TEMP"
lineSplit = split(line);
......@@ -835,10 +817,34 @@ classdef robotConnector < handle
obj.callbackFunction(obj,false,true,false,false);
end
end
elseif obj.lastCommand == "FILTER "+obj.filterID+" STATUS"
lineSplit = split(line);
if obj.linesWithLastCommand == 1
obj.filterInPos = str2double(lineSplit(8));
obj.filterOutPos = str2double(lineSplit(11));
obj.filterMinPos = str2double(lineSplit(15));
obj.filterMaxPos = str2double(lineSplit(18));
elseif obj.linesWithLastCommand == 2
obj.filterPos = str2double(lineSplit(7));
elseif obj.linesWithLastCommand == 3
if lineSplit(5) == 'TRUE'
obj.isEmergencyStopped = true;
elseif lineSplit(5) == 'FALSE'
obj.isEmergencyStopped = false;
else
error(strcat("Unknown bool '", string(lineSplit(5)), "', expected TRUE or FALSE"))
end
obj.commandIsRunning = false;
if obj.callbackEnabled
obj.callbackFunction(obj,false,false,false,true);
end
end
elseif obj.lastCommand == "FILTER IN" || ...
obj.lastCommand == "FILTER OUT" || ...
startsWith(obj.lastCommand,'FILTER POS ')
elseif obj.lastCommand == "FILTER "+obj.filterID+" IN" || ...
obj.lastCommand == "FILTER "+obj.filterID+" OUT" || ...
startsWith(obj.lastCommand,'FILTER '+obj.filterID+' POS ')
lineSplit = split(line);
if obj.linesWithLastCommand == 1
obj.filterGotoTarget = str2double(lineSplit(4));
......@@ -854,7 +860,7 @@ classdef robotConnector < handle
if obj.callbackEnabled
obj.callbackFunction(obj,true,true,false,true);
end
elseif startsWith(line, '< FILTER GO FINISHED POS =')
elseif startsWith(line, '< FILTER '+obj.filterID+' GO FINISHED POS =')
obj.filterGotoTarget = NaN;
obj.filterIsMoving = false;
obj.commandIsRunning = false;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment