101 codice C++ modificato per mimare il movimento di reaching: Main.cpp Appendice A

(1)

101

Appendice A

codice C++ modificato per mimare il movimento di reaching:

Main.cpp

//#include "./BoardSetup/TitechBoard.hpp" #include "./Common/Common.hpp" #include "./MainLoop/ControlLoop.hpp" #include "./MainLoop/MonitoringLoop.hpp" #include "./MainLoop/ActuatingLoop.hpp" #include "./ToolKit/Log/Logger.hpp" #include "./MainLoop/Loop.hpp" //#include "./Trajectory/Planner.hpp" #include "./SSSAPlanner/MotionGenerator.hpp" #include <ecl/threads.hpp> #include <ecl/time.hpp> #include <stdio.h> #include <iostream> using std::cerr; using std::endl; #include <fstream> using std::ofstream;

#include <cstdlib> // for exit function //#include <octave/oct.h>

//#include <octave/octave.h> //#include <octave/parse.h> //#include

//using namespace std;

//using namespace Octopus::BoardSetup; using namespace Octopus::Common;

using namespace Octopus::MainLoop; using namespace ECL::Threads; using ECL::Time::StopWatch; using ECL::Time::TimeStamp; using Octopus::ToolKit::Log::Logger; using Octopus::Common::sWait; //using Octopus::Trajectory::Planner; //using Octopus::SSSAPlanner::MotionGenerator; //using Octopus::SSSAPlanner::Loop; //using ECL::Time:: int main() {

(2)

102

// Loop *loop = ControlLoop::getInstance(logger);

// Loop *loop = MonitoringLoop::getInstance(logger); Loop *loop = ActuatingLoop::getInstance(logger); //MotionGenerator *motionGenerator =

MotionGenerator::getInstance(loop,logger); Thread loopThread(&Loop::start, *loop);

//Thread motionGeneratorThread(&MotionGenerator::start, *motionGenerator); bool stop = true;

char ch; //double inp; // wait(10); while(true) { if(!stop) {break;} scanf("%c",&ch); //scanf("%lf",&inp); switch (ch) { case 'a': { loop->ampl12=loop->ampl12-5;

printf("Ampiezza diminuita di un incremento\n"); break;

}

case 'l': {

loop->ampl12=loop->ampl12+5;

printf("Ampiezza accresciuta di un incremento\n"); break;

}

case 's': {

loop->ampl5=loop->ampl5-5;

}

case 'k': {

}

case 'd': {

}

case 'j': {

}

case 'f': {

(3)

103

printf("Ampiezza diminuita di un incremento\n");

break; }

case 'h': {

}

case 'z': {

}

case 'm': {

}

case 'x': {

}

case 'n': {

}

case 'c': {

}

case 'b': {

}

case 'w': {

}

case 'o': {

}

case 'e': {

(4)

104

}

case 'i': {

}

case 'r': {

}

case 'u': {

}

case 't': {

}

case 'y': {

}

/*case 'p': {

ofstream myfile ("example.txt"); if (myfile.is_open())

{

myfile << "This is a line.\n";

myfile << "This is another line.\n"; myfile.close();

}

else cout << "Unable to open file"; }*/

case 'q': {

// This program output values from a set of values to a file named ampiezze.dat

ofstream outdata; // outdata is like cin outdata.open("dati.dat"); // opens the file if( !outdata ) { // file couldn't be opened

cerr << "Error: file could not be opened" << endl; exit(1);

}

outdata << loop->ampl12 << " " << loop->ampl5<< " " << loop->ampl0 << " "<< loop->ampl7 << " "<< loop->ampl2 << " "<< loop->ampl10 << "

(5)

105

"<< loop->ampl8 << " "<< loop->ampl11 << " " << loop->ampl6 << " "<< loop->ampl1 << endl;

outdata.close();

stop = false;

printf("Breaking the loop\n"); break;

} }

// logger->writeValues(ctr++,inp);

/*printf("\n\nInput %lfN Received\n\n\n", inp); // if (ch=='q');

if (inp >= 0.00) {

printf("Breaking the loop\n"); break; }*/ } //motionGenerator->stop(); loop->stop(); //motionGenerator->remove(); loop->remove(); logger->remove(); //sWait(1); return 0; }

void stopWatchTest(int ctrLim) {

int ctr = 0;

StopWatch stopwatch; TimeStamp time;

printf("Checking wait\n"); long int loopTime = 250000;

long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev = 0; double avgTime; while(ctr++<ctrLim) { time = stopwatch.elapsed(); time1 = time.usec();

printf("Initial = %ld \t",time1); // wait(0.10);

(6)

106

uWait(50000);

time = stopwatch.elapsed(); time2 = time.usec();

printf("Final = %ld \t",time2); timeDiff = loopTime-(time2-time1);

printf("Must wait for %ld uSec \n", timeDiff); uWait(timeDiff);

time = stopwatch.elapsed(); time3 = time.usec();

// Job

totTime += time3 - time1;

avgTime = (double)totTime / (double)ctr;

printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

time1Prev = time1; stopwatch.restart(); } }

ActuatingLoop.cpp

#include "ActuatingLoop.hpp" #include <ecl/time.hpp> #include <stdio.h> #include "./../Common/Common.hpp" #include <math.h> #include <iostream> using std::cerr; using std::cout; using std::endl; #include <fstream> using std::ifstream;

#include <cstdlib> // for exit function

using Octopus::Common::sWait; using Octopus::Common::uWait; using Octopus::BoardSetup::TitechBoard; using ECL::Time::StopWatch; using ECL::Time::TimeStamp; using Octopus::ToolKit::Log::Logger;

(7)

107

//using ECL::Time:: namespace Octopus { namespace MainLoop { //ControlLoop * ControlLoop::controlLoop; //ActuatingLoop

Loop * ActuatingLoop::getInstance(Logger *log) { if(loop == NULL)

{

loop = new ActuatingLoop(log); }

return(loop); }

void ActuatingLoop::remove() {

printf("Removing Control Loop\n"); while(!loopFinished)

{

// uWait(10000); sWait(0.1);

printf("waiting for thread to finish\n"); } delete(this); } void ActuatingLoop::start() { //int ctr = 1; //StopWatch stopwatch; //TimeStamp time;

//long int loopTime = (long int)LOOP_TIME;

//long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev= 0;

//double avgTime; int lMeasDigital=0; int motorOutDigital= 0; // int pulseOut = 0;

double fMeasAnalog = 0.0;//, motorOutAnalog = 0.0; //, TsetPoint = 0.0, lastError = 0.0, errorDot = 0.0; vector<double>TsetPoint(MAX_NUM_OF_MOTOR, 0.0);

//double errorInt = 0.0; //double offset = 0.0; // bool initialising = true;

loopFinished = false; //Initialising Loop

(8)

108

while(true) { mutex.lock(); if(!continueLoop) {

printf("Loop trying to stop\n"); mutex.unlock(); break; } mutex.unlock(); //time = stopwatch.elapsed(); //time1 = time.usec(); // printf("Initial = %ld \t",time1); // wait(0.10); // uWait(50000); // Task // vector<int> temp(MAX_NUM_OF_SENSOR,0); vector<int> temp = titechBoard->readAD(); // printf("getting each sensor from vector ");

setPointMutex.lock(); TsetPoint = setPoints; setPointMutex.unlock(); // TsetPoint.at(0) = 0.0; // TsetPoint.at(1) = 0.0; // TsetPoint.at(2) = 0.0; //#if TRAJECTORY_TYPE ==1 // TsetPoint.at(1) = 1160 + (60 * sin((double)WAVE_FREQUENCY * timeCtr));

// TsetPoint.at(0) = 1138 + (38 * sin((double)WAVE_FREQUENCY * timeCtr + ((double)PI)));

// TsetPoint.at(0) = 1100 + (60 * sin((double)WAVE_FREQUENCY * timeCtr));

// TsetPoint.at(1) = 1170 + (60 * sin((double)WAVE_FREQUENCY * timeCtr + ((double)PI)));

//

// TsetPoint.at(2) = TsetPoint.at(0); // TsetPoint.at(3) = TsetPoint.at(1); //#else

// TsetPoint.at(1) = 40 + 20 * sin((double)WAVE_FREQUENCY * tempVal); // TsetPoint.at(2) = 40 + 20 * sin((double)WAVE_FREQUENCY * tempVal + ((double)PI/2));

//#endif

printf("fin qui j e %ld\n",j); // printf ("%Lf : ", timeCtr);

timeCtr = timeCtr + 0.05;

for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++) // for(int i=0; i<temp.size();i++)

{

lMeasDigital = temp.at(i);

// fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);

(9)

109

fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE *

(((double)lMeasDigital)/1023);

// motorOutDigital = (int)(1100 + 50 + 800*(((double)TsetPoint.at(i)*10)/(3.5*360)));

motorOutDigital = (int) TsetPoint.at(i);

if(motorOutDigital<1100) { motorOutDigital = 1100; } else if(motorOutDigital>1900) { motorOutDigital = 1900; }

// pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023)); // double thetaTemp = ((double)theta/1023);

// pulseOut = (int)(1100 + 800*((double)heta/1023)); // pulseOut = (int)(1100+ 800*(offset/1260));

// double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

// printf("%d: SP: %f, Act(N): %f, e: %f, eDot: %f, eInt: %f, KPe : %f, KDeDot : %f, KIeInt : %f", i, TsetPoint.at(i), fMeasAnalog, error.at(i), errorDot.at(i), errorInt.at(i), (double)K_P * error.at(i), (double)K_D *

errorDot.at(i), (double)K_I * errorInt.at(i));

// printf("%d: SP: %f, Act(N): %f ", i, (double)TsetPoint.at(i)*10, fMeasAnalog);

// printf("%d: Meas: %f, Act: %d ", i,fMeasAnalog , motorOutDigital);

if((i%2) ==0) {

printf("%d: Act: %d ", i, motorOutDigital); }

else {

printf("%d: Act: %d ", i, 1900 - (motorOutDigital-1100));

}

// titechBoard->writeMotor(i,pulseOut);

titechBoard->writeMotor(i,motorOutDigital); // printf("Sensor %d : %d ", i+1, temp.at(i+1));

}

//changeSetPointsintime(setPoints); changeSetPoints(setPoints);

j++;

(10)

110

// printf("\n"); // tempVal++; //time = stopwatch.elapsed(); //time2 = time.usec(); // printf("Final = %ld \t",time2);

//if(time2-time1 > 0 && (time2-time1) < loopTime) //{

//timeDiff = loopTime-(time2-time1); //}

//else //{

//printf("\ntime1 %ld time2 %ld looptime %ld",time1, time2, loopTime);

//printf("\nWtf ActuatingLoop thinks you were awesome! \n"); //}

// printf("Must wait for %ld uSec\n", timeDiff); //uWait(timeDiff);

//time = stopwatch.elapsed(); //time3 = time.usec();

// Job

//totTime += time3 - time1;

//avgTime = (double)totTime / (double)ctr; //ctr++;

// printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

//time1Prev = time1; //stopwatch.restart(); }

//Resetting motors

printf("\n \n Resetting \n");

for(int i=0; i<MAX_NUM_OF_MOTOR ;i++)

// for(int i=0; i<temp.size();i++) { // if(i%2==0){ // titechBoard->writeMotor(i,1100); // printf("%d: Act: %d\n", i, 1100); // } // else { // titechBoard->writeMotor(i,1900); // printf("%d: Act: %d\n", i, 1900); // } titechBoard->writeMotor(i,1100); } loopFinished = true;

(11)

111

//printf ("Average LoopTime was %f\n", avgTime);

printf("ControlLoop thread closing\n"); }

bool ActuatingLoop::stop() {

printf("Trying to stop Loop\n"); mutex.lock(); continueLoop = false; mutex.unlock(); return(continueLoop); } bool ActuatingLoop::finished() { return(loopFinished); } bool ActuatingLoop::changeSetPoints(vector<double> sp) { setPointMutex.lock(); setPoints = sp; setPointMutex.unlock(); if (j < 0) {

// This program reads values from the file 'example.dat' // and echoes them to the display until a negative value // is read.

ifstream indata; // indata is like cin

indata.open("dati.dat"); // opens the file if(!indata) { // file couldn't be opened

cerr << "Error: file could not be opened" << endl; exit(1);

}

indata >> ampl12 >> ampl5 >> ampl0 >> ampl7 >> ampl2 >> ampl10 >> ampl8 >> ampl11 >> ampl6 >> ampl1 ;

while ( !indata.eof() ) { // keep reading until end-of-file

cout << "The widths are " << ampl12 << ampl5 << ampl0 << ampl7 << ampl2 << ampl10 << ampl8 << ampl11 << ampl6 << ampl1 << endl;

indata >> ampl12 >> ampl5 >> ampl0 >> ampl7 >> ampl2 >> ampl10 >> ampl8 >> ampl11 >> ampl6 >> ampl1 ; // sets EOF flag if no value found

}

indata.close();

cout << "End-of-file reached.." << endl; printf("j: %ld\n",j);

sWait(2); }

//else if ((j = 0)) { else {

(12)

112

//setPoints.at(12) = 1160 + ampl12; //longitudinale alto

//setPoints.at(9) = 1130 + ampl9; //longitudinale basso float amp12 = 1200 + ampl12;

float amp9 = 1130 + ampl9; //float amp5 = 1130 + ampl5; //float amp0 = 1130 + ampl0; //float amp2 = 1130 + ampl2;

printf("larghezza impulso al longitudinale: %f\n",ampl12); //printf(" %f\n",a12); titechBoard->writeMotor(12,amp12); titechBoard->writeMotor(9,amp9); //titechBoard->writeMotor(5,amp5); //titechBoard->writeMotor(0,amp0); //titechBoard->writeMotor(2,amp2); printf("j: %ld\n",j); sWait(0.007875*900); amp12 = amp12 + 20; //float amp9 = 1130 + ampl9; //float amp5 = 1130 + ampl5; //float amp0 = 1130 + ampl0; //float amp2 = 1130 + ampl2;

//printf("larghezza impulso al longitudinale: %f\n",ampl12); //printf(" %f\n",a12); titechBoard->writeMotor(12,amp12); //titechBoard->writeMotor(9,amp9); //titechBoard->writeMotor(5,amp5); //titechBoard->writeMotor(0,amp0); //titechBoard->writeMotor(2,amp2); sWait(0.007875*300); amp12 = amp12 + 20;

//float amp9 = 1130 + ampl9; float amp5 = 1130 + ampl5; float amp0 = 1130 + ampl0; float amp2 = 1130 + ampl2;

//printf("larghezza impulso al longitudinale: %f\n",ampl12); //printf(" %f\n",a12); titechBoard->writeMotor(12,amp12); //titechBoard->writeMotor(9,amp9); titechBoard->writeMotor(5,amp5); titechBoard->writeMotor(0,amp0); titechBoard->writeMotor(2,amp2); sWait(0.007875*300); //titechBoard->writeMotor(12,1100); //sWait(0.007875*2000);

//setPoints.at(12) = 0; //longitudinale alto //setPoints.at(9) = 0; //longitudinale basso

//setPoints.at(5) = 1130 + ampl5; //longi-trasversale uno (prossimale) /*float amp5 = 1140 + ampl5;

printf("larghezza impulso al trasverso-longitudinale 1 : %f\n",ampl5); //printf(" %f\n",a5);

//titechBoard->writeMotor(12,1100); titechBoard->writeMotor(12,1190);

(13)

113

//titechBoard->writeMotor(9,1100);

titechBoard->writeMotor(5,amp5); //titechBoard->writeMotor(0,1107); sWait(0.007875*15);

//setPoints.at(0) = 1130 + ampl0; //longi-trasversale due float amp0 = 1147 + ampl0;

printf("larghezza impulso al trasverso-longitudinale 2 : %f\n",ampl0); //printf(" %f\n",a0); titechBoard->writeMotor(0,amp0); //titechBoard->writeMotor(12,1100); titechBoard->writeMotor(12,1150); //titechBoard->writeMotor(2,1123); sWait(0.007875*15);

//setPoints.at(7) = 1130 + ampl7; //longi-trasversale tre ---questo prima era 7

float amp2 = 1160 + ampl2;

titechBoard->writeMotor(2,amp2); titechBoard->writeMotor(12,1100); //titechBoard->writeMotor(7,1115); sWait(0.007875*20);*/

//setPoints.at(2) = 1150 + ampl2; //longi-trasversale quattro ---questo prima era 2

printf("larghezza impulso al trasverso-longitudinale 4 : %f\n",ampl7); //printf(" %f\n",a2); //titechBoard->writeMotor(12,1170); //titechBoard->writeMotor(9,1170); //titechBoard->writeMotor(12,1100); //titechBoard->writeMotor(9,1100); titechBoard->writeMotor(7,amp7); //titechBoard->writeMotor(10,1110); sWait(0.007875*20);

//setPoints.at(10) = 1150 + ampl10; //longi-trasversale cinque float amp10 = 1185 + ampl10;

printf("larghezza impulso al trasverso-longitudinale 5 : %f\n",ampl10); //printf(" %f\n",a10); titechBoard->writeMotor(10,amp10); //titechBoard->writeMotor(8,1130); //titechBoard->writeMotor(12,1100); //titechBoard->writeMotor(9,1100); sWait(0.007875*25);

//setPoints.at(8) = 1150 + ampl8; //longi-trasversale sei float amp8 = 1195 + ampl8;

(14)

114

//setPoints.at(11) = 1170 + ampl11; //longi-trasversale sette

//setPoints.at(6) = 1170 + ampl6; //longi-trasversale otto float amp6 = 1215 + ampl6;

//setPoints.at(1) = 1170 + ampl1; //longi-trasversale nove float amp1 = 1225 + ampl1;

titechBoard->writeMotor(1,amp1); sWait(0.007875*30);

//setPoints.at(3) = 1100 + ampl3; //longitudinale sinistro //setPoints.at(4) = 1100 + ampl4; //longitudinale destro float amp3 = 1100 + ampl3;

titechBoard->writeMotor(3,amp3); titechBoard->writeMotor(4,amp4); sWait(0.007875*90); titechBoard->writeMotor(12,1100); titechBoard->writeMotor(5,1100); titechBoard->writeMotor(0,1100); titechBoard->writeMotor(7,1100); titechBoard->writeMotor(2,1100); titechBoard->writeMotor(10,1100); titechBoard->writeMotor(8,1100); titechBoard->writeMotor(11,1100); titechBoard->writeMotor(6,1100); titechBoard->writeMotor(1,1100); sWait(0.007875*500); } return(true); } bool ActuatingLoop::changeSetPointsintime(vector<double>st){

(15)

115

setPointMutex.lock();

setPoints = st;

setPointMutex.unlock(); //control starts here if (j < 0)

{

setPoints.at(12) = 1200 + ampl12; //longitudinale alto gia con un input setPoints.at(9) = 1100 + ampl9; //longitudinale basso

sWait(0.007875*800);

printf("retroflessione %f\n",ampl12); }

else {

setPoints.at(12) = 0; //longitudinale alto setPoints.at(9) = 0; //longitudinale basso

if ((setPoints.at(5)) >= 1100 && (setPoints.at(5)) < 1190) //valore troppo alto, solo per prova...

{ setPoints.at(5) = 1100 + ampl5 + (j*5); }//longi-trasversale uno (prossimale)

else if ((setPoints.at(5)) < 1100){ setPoints.at(5) = 1100;} else {

setPoints.at(5) = 1190;}

if (setPoints.at(0) >= 1100 && setPoints.at(0) < 1180){ //qui il limite va prima messo alto sennò è inutile avere la prima parte

setPoints.at(0) = 1100 + ampl0 + ((j*5)- 20); }//longi-trasversale due

else if (setPoints.at(0) < 1100) { setPoints.at(0) = 1100; } else {

if ((setPoints.at(7)) >= 1100 && (setPoints.at(7)) < 1160){

setPoints.at(7) = 1100 + ampl7 + ((j*5) - 40); }//longi-trasversale tre

if ((setPoints.at(7)) < 1100) { setPoints.at(7) = 1100; } else {

if (setPoints.at(2) >= 1100 && (setPoints.at(2)) < 1175){

setPoints.at(2) = 1100 + ampl2 + ((j*5) - 60); }//longi-trasversale quattro

if ((setPoints.at(10)) >= 1100 && (setPoints.at(10)) < 1185){ setPoints.at(10) = 1100 + ampl10 + ((j*5) - 80); } //longi-trasversale cinque

if ((setPoints.at(10)) < 1100) { setPoints.at(10) = 1100; }

(16)

116

else {

if ((setPoints.at(8)) >= 1100 && (setPoints.at(8)) < 1195){ setPoints.at(8) = 1100 + ampl8 + ((j*5) - 100); } //longi-trasversale sei

if ((setPoints.at(11)) >= 1100 && (setPoints.at(11)) < 1205){ setPoints.at(11) = 1100 + ampl11 + ((j*5) - 120); } //longi-trasversale sette

if ((setPoints.at(6)) >= 1100 && (setPoints.at(6)) < 1215){ setPoints.at(6) = 1100 + ampl6 + ((j*5) - 140); } //longi-trasversale otto

if ((setPoints.at(1)) >= 1100 && (setPoints.at(1)) < 1220){ setPoints.at(1) = 1100 + ampl1 + ((j*5) - 160); } //longi-trasversale nove (distale)

setPoints.at(3) = 1100 + ampl3; //longitudinale sinistro setPoints.at(4) = 1100 + ampl4; //longitudinale destro

printf("cambio di tempo %f\n",ampl0); sWait(0.007875*20);

printf("j: %ld\n",j); }

return (true); }

ActuatingLoop::ActuatingLoop(Logger *log) : sensorValues(MAX_NUM_OF_SENSOR,0), motorCommands(MAX_NUM_OF_MOTOR,0), setPoints(MAX_NUM_OF_MOTOR,0.0) { logger = log; titechBoard = TitechBoard::getInstance(); continueLoop = true; loopFinished = true; offsetForce = 0.0;

(17)

117

printf("Created New ControlLoop\n");

setPointsChanged = false; ampl0=0.0; ampl1=0.0; ampl2=0.0; ampl3=0.0; ampl4=0.0; ampl5=0.0; ampl6=0.0; ampl7=0.0; ampl8=0.0; ampl9=0.0; ampl10=0.0; ampl11=0.0; ampl12=0.0; j=-1; } ActuatingLoop::~ActuatingLoop() { titechBoard->remove(); } } }