Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

cleaned alot up in the serial code

  • Loading branch information...
commit 67ecd4fd96ec06beaee5e97a20ae1cf9af944532 1 parent a5c2e28
@HalfdanJ HalfdanJ authored
View
106 simpleExample/testApp.cpp
@@ -4,7 +4,7 @@
void testApp::setup(){
industrialRobot = new ofxIndustrialRobot(this);
industrialRobot->setInput(ofxIndustrialRobot::Gravity);
- industrialRobot->gotoStartPosition();
+ industrialRobot->gotoResetPosition();
}
//--------------------------------------------------------------
@@ -36,114 +36,18 @@ void testApp::update(){
industrialRobot->thread.unlock();
}*/
- unsigned char b1=0;
- for(int i=0;i<8;i++){
- if(byteone[i]){
- b1 |= 0x01<<i;
- }
- }
-
- unsigned char b2=0;
- for(int i=0;i<8;i++){
- if(bytetwo[i]){
- b2 |= 0x01<<i;
- }
- }
-
- if(industrialRobot->thread.serial->lock()){
- industrialRobot->thread.serial->outbuf[24-3] = b1;
- // cout<<(unsigned char )b1<<" "<<(unsigned char )b2<<endl;
- industrialRobot->thread.serial->outbuf[24-2] = b2;
-
+ if(industrialRobot->getSerial()->lock()){
for(int i=0;i<8;i++){
- if(industrialRobot->thread.serial->returnedFlags[i])
+ if(industrialRobot->getSerial()->returnedFlags[i])
bytestatus[i] = true;
else
bytestatus[i] = false;
}
-
- if(resetting > 0){
- switch (resetting) {
- case 1:
- unlockMotors = false;
- resetting++;
- byteone[5] = true;
- ofSleepMillis(20);
- break;
-
- case 2:
- byteone[5] = false;
- resetting++;
- break;
- case 3:
- if(bytetwo[1]){
- if(bytestatus[1]){
- resetting ++;
- }
- } else {
- bytetwo[1] = true;
- unlockMotors = (true);
- }
- break;
- case 4:
- resetting++;
- break;
- case 5:
- if(bytetwo[3]){
- if(bytestatus[3] && bytestatus[4]){
- resetting ++;
- }
- } else {
- bytetwo[3] = true;
- bytetwo[4] = true;
- }
- break;
- case 6:
- if(bytetwo[2]){
- if(bytestatus[2]){
- resetting ++;
- }
- } else {
- bytetwo[2] = true;
- // ofSleepMillis(10);
- }
- break;
- case 7:
- if(bytetwo[0]){
- if(bytestatus[0]){
- resetting ++;
- }
- } else {
- bytetwo[0] = true;
- }
- break;
- case 8:
- for(int i=0;i<5;i++){
- bytetwo[i] = false;
- }
- industrialRobot->thread.controller->gotoResetPosition();
- for(int i=0;i<5;i++){
- //motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation*100.0);
- }
- resetting = 0;
- break;
- default:
- break;
- }
-
- }
-
- /* if(industrialRobot->thread.serial->connected){
- resetMotors->setEnabled(true);
- unlockMotors->setEnabled(true);
- } else {
- resetMotors->setEnabled(false);
- unlockMotors->setEnabled(false);
- }*/
+
for(int i=0;i<5;i++){
string l;
- if(industrialRobot->thread.serial->connected){
+ if(industrialRobot->getSerial()->connected){
l = "Motor "+ofToString(i,0)+": ";
if(!byteone[7]){
l+= "LOCKED ";
View
14 src/ofxIndustrialRobot.cpp
@@ -20,6 +20,14 @@ ofxIndustrialRobot::ofxIndustrialRobot(ofBaseApp * _app){
}
+ofxIndustrialRobotSerial * ofxIndustrialRobot::getSerial(){
+ return thread.serial;
+}
+
+void ofxIndustrialRobot::gotoResetPosition(){
+ thread.controller->gotoResetPosition();
+}
+
void ofxIndustrialRobot::gotoStartPosition(){
/*if(rotations[0] < -90){
@@ -390,4 +398,10 @@ void ofxIndustrialRobot::panic(string msg){
thread.panicMessage = msg;
thread.panic = true;
}
+
+void ofxIndustrialRobot::resetRobot(){
+ getSerial()->resetting = 1;
+ gotoResetPosition();
+}
+
//---
View
8 src/ofxIndustrialRobot.h
@@ -73,6 +73,14 @@ class ofxIndustrialRobot {
void panic(string msg);
void gotoStartPosition();
+
+ //The position the robot is in after a reset
+ void gotoResetPosition();
+
+ ofxIndustrialRobotSerial * getSerial();
+
+ void resetRobot();
+
bool startReady;
bool startPhase2;
View
215 src/ofxIndustrialRobotSerial.cpp
@@ -1,14 +1,10 @@
- #include "ofxIndustrialRobotSerial.h"
+#include "ofxIndustrialRobotSerial.h"
ofxIndustrialRobotSerial::ofxIndustrialRobotSerial(){
serial.setVerbose(false);
connected = serial.setup("/dev/tty.usbserial-A7007bv1", 115200);
- nTimesRead = 0;
- nBytesRead = 0;
- readTime = 0;
- memset(bytesReadString, 0, 4);
for(int i=0;i<5;i++){
inValues[i] = 0;
@@ -18,27 +14,29 @@ ofxIndustrialRobotSerial::ofxIndustrialRobotSerial(){
dptr = 0;
botStatus = 0;
hasReceivenLastPosition = false;
- //goodCounter = 0;
errorCounter = 0;
+
+ for(int i=0;i<8;i++){
+ sendFlags[0][i] = false;
+ sendFlags[1][i] = false;
+ }
+
+ resetting = 0;
}
void ofxIndustrialRobotSerial::start(){
startThread(true, false); // blocking, verbose
}
-void ofxIndustrialRobotSerial::setValue(int n, float val){
- outValues[n] = val;
-}
+
void ofxIndustrialRobotSerial::sendValues(){
bool ok = true;
if(connected){
- //((float*)outbuf)[3]=(float)(sin(ofGetElapsedTimeMillis()/1000.0)*TWO_PI);
for(int i=0;i<num_axis;i++){
- if(outValues[i] != outValues[i]){
- // cout<<"ARHRHRHIUAOSHDIUASHDIUH"<<endl;
- ok =false;
+ if(outValues[i] != outValues[i]){ //Ehh?
+ ok = false;
}
if(outValues[i] > 5000){
outValues[i] = 5000;
@@ -46,18 +44,31 @@ void ofxIndustrialRobotSerial::sendValues(){
if(outValues[i] < -5000){
outValues[i] = -5000;
}
-// cout<<outValues[i] <<endl;
((float*)outbuf)[i]=outValues[i];
}
+
+ unsigned char b1=0;
+ for(int i=0;i<8;i++){
+ if(sendFlags[0][i]){
+ b1 |= 0x01<<i;
+ }
+ }
+ unsigned char b2=0;
+ for(int i=0;i<8;i++){
+ if(sendFlags[1][i]){
+ b2 |= 0x01<<i;
+ }
+ }
+
+ outbuf[DATAGRAM_LENGTH-4]='*';
+ outbuf[DATAGRAM_LENGTH-3] = b1;
+ outbuf[DATAGRAM_LENGTH-2] = b2;
- if(ok){
- outbuf[DATAGRAM_LENGTH-4]='*';
-
- for(int i=0;i<DATAGRAM_LENGTH;i++){
- sendingoutbuf[i] = outbuf[i];
- }
-
- serial.writeBytes(sendingoutbuf, DATAGRAM_LENGTH);
+ if(ok){
+ for(int i=0;i<DATAGRAM_LENGTH;i++){
+ sendingoutbuf[i] = outbuf[i];
+ }
+ serial.writeBytes(sendingoutbuf, DATAGRAM_LENGTH);
}
}
@@ -76,6 +87,76 @@ void ofxIndustrialRobotSerial::step(){
sendValues();
}
// cout<<"In: "<<inValues[1]<<endl;
+
+ if(resetting > 0){
+ switch (resetting) {
+ case 1: //Reset power
+ setUnlockFlag(false);
+ resetting++;
+ setResetPowerFlag(true);
+ ofSleepMillis(20);
+ break;
+ case 2: //Disable reset power
+ setResetPowerFlag(false);
+ resetting++;
+ break;
+ case 3: //Reset 1
+ if(resetMotorFlag(1)){
+ if(motorFlag(1)){ //Check if ok
+ resetting ++;
+ }
+ } else {
+ setResetMotorFlag(1, true);
+ setUnlockFlag(true);
+ }
+ break;
+ case 4: //Nothing
+ resetting++;
+ break;
+ case 5: //reset 3 & 4
+ if(resetMotorFlag(3)){
+ if(motorFlag(3) && motorFlag(4)){
+ resetting ++;
+ }
+ } else {
+ setResetMotorFlag(3, true);
+ setResetMotorFlag(4, true);
+ }
+ break;
+ case 6: //Reset 2
+ if(resetMotorFlag(2)){
+ if(motorFlag(2)){
+ resetting ++;
+ }
+ } else {
+ setResetMotorFlag(2, true);
+ // ofSleepMillis(10);
+ }
+ break;
+ case 7:
+ if(resetMotorFlag(0)){
+ if(motorFlag(0)){
+ resetting ++;
+ }
+ } else {
+ setResetMotorFlag(0, true);
+ }
+ break;
+ case 8:
+ for(int i=0;i<5;i++){
+ setResetMotorFlag(i, false);
+ }
+ //industrialRobot->thread.controller->gotoResetPosition();
+ for(int i=0;i<5;i++){
+ //motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation*100.0);
+ }
+ resetting = 0;
+ break;
+ default:
+ break;
+ }
+
+ }
}
}
@@ -125,39 +206,39 @@ void ofxIndustrialRobotSerial::recvChar(char c){
{
if(INBUF[DATAGRAM_LENGTH-4]=='*')
{
-
- botStatus = INBUF[DATAGRAM_LENGTH-1];
- for(int i =0;i<num_axis;i++){
- // cout<<inValues[i]<<endl;
- inValues[i] = ((float*)INBUF)[i];
- }
- //HAPPY
- if(!hasReceivenLastPosition){
- for(int i =0;i<num_axis;i++){
- initValues[i] = inValues[i];
- }
- hasReceivenLastPosition = true;
-
- }
- // printf("%02x\r\n",botStatus);
- for(int i=0;i<8;i++){
- if(botStatus & 0x01<<i)
- returnedFlags[i] = true;
- else
- returnedFlags[i] = false;
- }
-
- if(botStatus != 0x1f){
+
+ botStatus = INBUF[DATAGRAM_LENGTH-1];
+ for(int i =0;i<num_axis;i++){
+ // cout<<inValues[i]<<endl;
+ inValues[i] = ((float*)INBUF)[i];
+ }
+ //HAPPY
+ if(!hasReceivenLastPosition){
+ for(int i =0;i<num_axis;i++){
+ initValues[i] = inValues[i];
+ }
+ hasReceivenLastPosition = true;
+
+ }
+ // printf("%02x\r\n",botStatus);
+ for(int i=0;i<8;i++){
+ if(botStatus & 0x01<<i)
+ returnedFlags[i] = true;
+ else
+ returnedFlags[i] = false;
+ }
+
+ if(botStatus != 0x1f){
// printf("%0x2f:%0x2f:%0x2f:%0x2f\r\n",INBUF[DATAGRAM_LENGTH-4],INBUF[DATAGRAM_LENGTH-3],INBUF[DATAGRAM_LENGTH-2],INBUF[DATAGRAM_LENGTH-1]);
- }
+ }
dptr=0;
-
-
+
+
}
else
{
- // goodCounter = 0;
+ // goodCounter = 0;
cout<<"Error in serialcommunication nr. "<<errorCounter<<endl;
errorCounter++;
if(INBUF[DATAGRAM_LENGTH-5]=='*'){
@@ -186,16 +267,50 @@ bool ofxIndustrialRobotSerial::panicFlag(){
bool ofxIndustrialRobotSerial::lockFlag(){
return !outbuf[24-3];
+}
+
+bool ofxIndustrialRobotSerial::resetMotorFlag(int n){
+ return sendFlags[1][n];
+}
+
+bool ofxIndustrialRobotSerial::speedLimitFlag(){
+ return sendFlags[0][0];
+}
+bool ofxIndustrialRobotSerial::resetPowerFlag(){
+ return sendFlags[0][5];
+}
+bool ofxIndustrialRobotSerial::unlockFlag(){
+ return sendFlags[0][7];
+}
+
+void ofxIndustrialRobotSerial::setValue(int n, float val){
+ outValues[n] = val;
+}
+
+void ofxIndustrialRobotSerial::setSpeedLimitFlag(bool flag){
+ sendFlags[0][0] = flag;
+}
+void ofxIndustrialRobotSerial::setResetPowerFlag(bool flag){
+ sendFlags[0][5] = flag;
}
+void ofxIndustrialRobotSerial::setUnlockFlag(bool flag){
+ sendFlags[0][7] = flag;
+}
+
+void ofxIndustrialRobotSerial::setResetMotorFlag(int motor, bool flag){
+ sendFlags[1][motor] = flag;
+}
+
+
void ofxIndustrialRobotSerial::threadedFunction(){
while( isThreadRunning() != 0 ){
if( lock() ){
// count++;
-// lastStep = ofGetElapsedTimeMillis();
+ // lastStep = ofGetElapsedTimeMillis();
step();
unlock();
ofSleepMillis(8);
View
99 src/ofxIndustrialRobotSerial.h
@@ -4,42 +4,73 @@
#include "ofxIndustrialRobotMotorConverter.h"
#define DATAGRAM_LENGTH 24
+/*
+ Description of the bytes:
+
+ byte 0:
+ 0: No speed limit
+ 1: bit 1 (?)
+ 2: bit 2 (?)
+ 3: bit 3 (?)
+ 4: bit 4 (?)
+ 5: Reset power
+ 6: Run 0 (?)
+ 7: Run 1 (Unlock motors)
+
+ byte 1:
+ 0: Reset motor 0
+ 1: Reset motor 1
+ 2: Reset motor 2
+ 3: Reset motor 3
+ 4: Reset motor 4
+ 5: bit 5 (?)
+ 6: bit 6 (?)
+ 7: bit 7 (?)
+
+ byte status:
+ 0: Axis 0 OK
+ 1: Axis 1 OK
+ 2: Axis 2 OK
+ 3: Axis 3 OK
+ 4: Axis 4 OK
+ 5: Bit 5 (?)
+ 6: Manual stop (emergency stop)
+ 7: Hardware error (panic)
+ */
+
class ofxIndustrialRobotSerial : public ofxThread{
public:
- ofSerial serial;
-
-
ofxIndustrialRobotSerial();
- void step();
-
- bool bSendSerialMessage; // a flag for sending serial
- char bytesRead[1]; // data from serial, we will be trying to read 3
- char bytesReadString[4]; // a string needs a null terminator, so we need 3 + 1 bytes
- int nBytesRead; // how much did we read?
- int nTimesRead; // how many times did we read?
- float readTime; // when did we last read?
-
- static const int num_axis = 5;
- float inValues[5];
- float initValues[5];
- float outValues[5];
+
bool returnedFlags[8];
-
+ bool sendFlags[2][8];
+
+ bool hasReceivenLastPosition;
+
bool motorFlag(int n);
bool emergencyFlag();
bool panicFlag();
bool lockFlag();
-
+
+ bool resetMotorFlag(int n);
+ bool speedLimitFlag();
+ bool resetPowerFlag();
+ bool unlockFlag();
+
+ void setSpeedLimitFlag(bool flag);
+ void setResetPowerFlag(bool flag);
+ void setUnlockFlag(bool flag);
+ void setResetMotorFlag(int motor, bool flag);
+
+ void step();
void recvChar(char c);
- bool hasReceivenLastPosition;
void setValue(int n, float val);
void sendValues();
-
bool connected;
void start();
@@ -48,21 +79,35 @@ class ofxIndustrialRobotSerial : public ofxThread{
stopThread();
}
-
+
+ int resetting;
//--------------------------
void threadedFunction();
- unsigned char outbuf[DATAGRAM_LENGTH];
- unsigned char sendingoutbuf[DATAGRAM_LENGTH];
-
- unsigned int lastStep;
-
- int errorCounter;
+
+
private:
unsigned char syncstate;
unsigned char dptr;
unsigned char INBUF[DATAGRAM_LENGTH];
unsigned char botStatus;
+
+ unsigned char sendingoutbuf[DATAGRAM_LENGTH];
+ unsigned int lastStep;
+
+ int errorCounter;
+
+ ofSerial serial;
+
+ int inValues[5];
+ int outValues[5];
+ float initValues[5];
+
+ static const int num_axis = 5;
+ unsigned char outbuf[DATAGRAM_LENGTH];
+
+
+
};
Please sign in to comment.
Something went wrong with that request. Please try again.