diff --git a/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur.ino b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur.ino new file mode 100644 index 0000000..5f1277e --- /dev/null +++ b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur.ino @@ -0,0 +1,715 @@ +#include "io.h" +#include // Include the Metro library +#include "com.h" + +Metro newTest = Metro(1); + +#include // Define a stepper and the pins it will use +#define NBMOTEURS 6 +//#define SerialUSB Serial1 // to change instance SerialUSB as Serial1 + +#define NBPASPARTOUR 6400 // Set on the driver + +#define STEP 1//// 800-->Mode 1/4 pas MS2 sur ON +// 1600-->Mode 1/8 MS1+ MS2 sur ON +// 3200--> Mode 1/16 MS3 sur ON +// 6400--> Mode 1/32 MS1+ MS3 sur ON + + //******* From the behind to the front +// 6400 Arduino Due +// const uint8_t PINDIRECTION[NBMOTEURS] = { 52, 48, 44, 40, 36, 32, 28, 24, 21, 17 };//{ 10, 6, 2, 22, 26}; +// const uint8_t PINSPEED[NBMOTEURS]= { 53, 49, 45, 41, 37, 33, 29, 25, 20, 16 }; //{ 11, 7, 3, 23, 27}; + + +// 6400 Teensy + const uint8_t PINDIRECTION[NBMOTEURS] = {6, 9, 12, 26, 29, 32}; + const uint8_t PINSPEED[NBMOTEURS]= {5, 8, 11, 25, 28, 31}; + + const uint8_t ENABLEPIN[NBMOTEURS]= {4, 7, 10, 24, 27, 30}; + + +// Define a stepper and the pins it will use +AccelStepper stepper[ NBMOTEURS] = { +AccelStepper (STEP, PINSPEED[0], PINDIRECTION[0]), +AccelStepper (STEP, PINSPEED[1], PINDIRECTION[1]), +AccelStepper (STEP, PINSPEED[2], PINDIRECTION[2]), +AccelStepper (STEP, PINSPEED[3], PINDIRECTION[3]), +AccelStepper (STEP, PINSPEED[4], PINDIRECTION[4]), +AccelStepper (STEP, PINSPEED[5], PINDIRECTION[5]), + +}; + +// Receive with start- and end-markers combined with parsing + +const byte numChars = 200; +char receivedChars[numChars]; +char tempChars[numChars]; // temporary array for use when parsing + + // variables to hold the parsed data +char messageFromPC[numChars] = {0}; //or 5 doesn't change anything + +int integerFromPC0 = 0; +int integerFromPC1 = 0; +int integerFromPC2 = 0; +int integerFromPC3 = 0; +int integerFromPC4 = 0; +int integerFromPC5 = 0; +int integerFromPC6 = 0; +int integerFromPC7 = 0; +int integerFromPC8 = 0; +int integerFromPC9 = 0; + +int PC0= 0; +int PC1= 0; +int PC2= 0; +int PC3= 0; +int PC4= 0; +int PC5= 0; +int PC6= 0; +int PC7= 0; +int PC8= 0; +int PC9= 0; + + +int PCTer0= 0; +int PCTer1= 0; +int PCTer2= 0; +int PCTer3= 0; +int PCTer4= 0; +int PCTer5= 0; +int PCTer6= 0; +int PCTer7= 0; +int PCTer8= 0; +int PCTer9= 0; +int PCTer10= 0; +int PCTer11= 0; + +int orderTrigger = 0; +int orderCohesion = 0; +int orderCohesionB = 0; +int startStop = 0; + +int led = 13; + +float floatFromPC = 0.0; // not used for the moment + +boolean newData = false; +//==================================================================== CHECK NUMBEROF ROUND AND GOOD MOVEMENT + + +int tourTest; +// IntervalTimer t1; + +#define TX_SIZE 512 +uint8_t tx_buffer[TX_SIZE]; + +//================================================================== Jonathan computingDelta between actual and previous position. + +int processingPosition, processingPosition1,processingPosition2, processingPosition3, processingPosition4; +int processingPosition5, processingPosition6, processingPosition7, processingPosition8, processingPosition9; + + +int positionX, positionX1, positionX2, positionX3,positionX4,positionX5, positionX6, positionX7, positionX8, positionX9; +int previousX, previousX1; // positionX precedente + + +int16_t computeDeltaPosition( uint16_t processingPosition) { //Moteur 11, le plus à droite des drivers + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR*1; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR*1/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR*1; + oldPositionAbsolue = processingPosition; + return resultat; + +} + +int16_t computeDeltaPosition1( uint16_t processingPosition1) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition1; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR*1; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR*1/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR*1; + oldPositionAbsolue = processingPosition1; + return resultat; + +} + +int16_t computeDeltaPosition2( uint16_t processingPosition2) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition2; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR*1; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR*1/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR*1; + oldPositionAbsolue = processingPosition2; + return resultat; + +} + + +int16_t computeDeltaPosition3( uint16_t processingPosition3) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition3; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition3; + return resultat; + +} +int16_t computeDeltaPosition4( uint16_t processingPosition4) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition4; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition4; + return resultat; + +} + +int16_t computeDeltaPosition5( uint16_t processingPosition5) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition5; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition5; + return resultat; + +} + +int16_t computeDeltaPosition6( uint16_t processingPosition6) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition6; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition6; + return resultat; + +} + +int16_t computeDeltaPosition7( uint16_t processingPosition7) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition7; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition7; + return resultat; + +} +int16_t computeDeltaPosition8( uint16_t processingPosition8) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition8; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition8; + return resultat; + +} + +int16_t computeDeltaPosition9( uint16_t processingPosition9) { + static uint16_t oldPositionAbsolue = 0; + uint16_t positionAbsolue = processingPosition9; + int16_t resultat; + if(positionAbsolue < oldPositionAbsolue) + positionAbsolue+=NBPASPARTOUR; + if(positionAbsolue - oldPositionAbsolue < NBPASPARTOUR/2) + resultat = positionAbsolue - oldPositionAbsolue; + else + resultat = positionAbsolue - oldPositionAbsolue - NBPASPARTOUR; + oldPositionAbsolue = processingPosition9; + return resultat; + +} + +void setup() { + initEncoders(); + // initIo(); + // initCom(); + // initSteppers(); + // printSerialCommandsAvailable(); + + + // initialize digital pin LED_BUILTIN as an output. + + + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); + // pinMode(led, OUTPUT); + + // It's to fix the good period time between two ticks + // t1.priority(16); //32 + //I have to change this value in the file serial.c. + //Later we will can change this directly in the setup + //This file is a folder in the application teensy +// NVIC_SET_PRIORITY(IRQ_UART0_STATUS, 16); //32 + +// t1.begin(tickSteppers, 100 ); // call every 100µs, change if requred OK. If it's shorter it's preciser + // I don't use this fonction but i let it if i need + + processingPosition = PC0; + processingPosition1 = PC1; + processingPosition2 = PC2; + processingPosition3 = PC3; + processingPosition4 = PC4; + processingPosition5 = PC5; + processingPosition6 = PC6; + processingPosition7 = PC7; + processingPosition8 = PC8; + processingPosition9 = PC9; //K + + Serial.begin (115200); + Serial1.begin (115200); + // SerialUSB.begin (115200); + + for(uint8_t i = 0; i < NBMOTEURS; i++) { + + // Initialisation des pins moteurs + pinMode(ENABLEPIN[i], OUTPUT); + digitalWrite(ENABLEPIN[i], OUTPUT); + digitalWrite(ENABLEPIN[i], LOW); + + pinMode(PINDIRECTION[i], OUTPUT); + digitalWrite(PINDIRECTION[i], OUTPUT); + pinMode(PINSPEED[i], OUTPUT); + digitalWrite(PINSPEED[i], OUTPUT); + // stepper[i].setMinPulseWidth(5); //****************************************ADD WITH TEENSY if clock very fast + + // stepper[i].setMaxSpeed(25000); // 11000 + //*** stepper[i].setMaxSpeed(60000/2); // 11000 + stepper[i].setMaxSpeed(6400*2.5); // 11000 + // stepper[i].setMaxSpeed(NBPASPARTOUR*20); // 11000 + // stepper[i].setAcceleration(12800); // 12000 + // stepper[i].setMaxSpeed(19800); // 3.5 round/s +// stepper[i].setMaxSpeed(3200); // 3.5 round/s + +// stepper[i].setAcceleration(NBPASPARTOUR*5); // 12000 +//*** stepper[i].setAcceleration(40000/2); // 12000 + stepper[i].setAcceleration(6400*1); //tmc 2209 + // stepper[i].setAcceleration(6400*3); // augmente acceleration avec DM556 + // stepper[i].setAcceleration(12000); + // stepper[i].setAcceleration(4800); + // stepper[i].setAcceleration(3200); // poiur satie + + } + + tourTest=6400*1; + /* + positionX9= tourTest*2; // premier + + positionX8=tourTest*3; // DM556 + positionX7=tourTest; + + positionX6=tourTest; + + positionX5=tourTest; + positionX4=tourTest*3; + + positionX3=tourTest*1; + positionX2=tourTest*1; + positionX1=tourTest; + + positionX=tourTest*-3; // premier devant moi +*/ + +//PCTer1=1; //off + PCTer1=3; //on + + +// PCTer3=1; //off + PCTer3=3; //on + + + PCTer2= 2; // JoeTransfo + PCTer2= -2; // noJoeTransfo + + PC0=tourTest*1; // premier devant moi + + PC9=PC0; //PC9 dernier moteur + PC8=PC0; + PC7=PC0; + PC6=PC0; + PC5=PC0; + PC4=PC0; + PC3=PC0; + PC2=PC0; + PC1=PC0; //PC0 premier moteur + + PCTer2 = -3; //noJoe + } + + +void loop() { +// digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level) +// delay(200); // wait for a second +// digitalWrite(led, LOW); // turn the LED off by making the voltage LOW +// delay(200); + + displayAllCountSerial1(); + + if (PCTer1 > 0) { + digitalWrite(ENABLEPIN[0], LOW); // you turn on the driver + } + if (PCTer1 < 1) { + digitalWrite(ENABLEPIN[0], HIGH); // you turn off the driver + } + if (PCTer3 > 0) { + digitalWrite(ENABLEPIN[1], LOW); // you turn on the driver + } + if (PCTer3 < 1) { + digitalWrite(ENABLEPIN[1], HIGH); // you turn off the driver + } + + + + for(uint8_t i = 0; i < NBMOTEURS; i++) { + + // stepper[i].setMinPulseWidth(5); //****************************************IN SETUP ADD WITH TEENSY if clock very fast + + stepper[i].setAcceleration(1600*PCTer0); //tmc 2209 + + } + + readDataFromProcessingToSerial1(); // READ 30 datas even if there is ony 24 datas from Processing + + + recvWithStartEndMarkers(); // recevoir chaine de character et disperse en unité + if (newData == true) { + strcpy(tempChars, receivedChars); + // this temporary copy is necessary to protect the original data + // because strtok() used in parseData() replaces the commas with \0 + parseData(); + + + newData = false; + + } + if (PCTer2 < -1) { // noJoTransformation Lire les données telles qu'elles arrivent depuis Processing + noJoeTransformation(); + } + else if (PCTer2 > -1) { // transforme data avec la methode de Jo + + processingPosition = PC0; + processingPosition1 = PC1; + processingPosition2 = PC2;//R + processingPosition3 = PC3; + processingPosition4 = PC4; + processingPosition5 = PC5; + processingPosition6 = PC6; + processingPosition7 = PC7; + processingPosition8 = PC8; + processingPosition9 = PC9;//K + + positionX += computeDeltaPosition(processingPosition); + positionX1+= computeDeltaPosition1(processingPosition1); + positionX2+= computeDeltaPosition2(processingPosition2); + positionX3+= computeDeltaPosition3(processingPosition3); + positionX4+= computeDeltaPosition4(processingPosition4); + positionX5+= computeDeltaPosition5(processingPosition5); + positionX6+= computeDeltaPosition6(processingPosition6); + positionX7+= computeDeltaPosition7(processingPosition7); + positionX8+= computeDeltaPosition8(processingPosition8); + positionX9+= computeDeltaPosition9(processingPosition9); + + /* + stepper[9].moveTo(-positionX9); + stepper[9].run(); + + stepper[8].moveTo(-positionX8); + stepper[8].run(); + + stepper[7].moveTo(-positionX7); + stepper[7].run(); + + stepper[6].moveTo(-positionX6); + stepper[6].run(); + */ + stepper[5].moveTo(-positionX5); // + stepper[5].run(); + + stepper[4].moveTo(-positionX4); + stepper[4].run(); + + stepper[3].moveTo(-positionX3); + stepper[3].run(); + + stepper[2].moveTo(-positionX2); + stepper[2].run(); + + stepper[1].moveTo(-positionX1); + stepper[1].run(); + + + stepper[0].moveTo(-positionX); // premiere donnée envoyée dansla chaine de String de Processing + stepper[0].run(); // plus pres de moi. oscillator 11 + + + + } +} + +void recvWithStartEndMarkers() { + static boolean recvInProgress = false; + static byte ndx = 0; + char startMarker = '<'; + char endMarker = '>'; + char rc; + + while (Serial.available() > 0 && newData == false) { + rc = Serial.read(); + + if (recvInProgress == true) { + if (rc != endMarker) { + receivedChars[ndx] = rc; + ndx++; + if (ndx >= numChars) { + ndx = numChars - 1; + } + } + else { + receivedChars[ndx] = '\0'; // terminate the string + recvInProgress = false; + ndx = 0; + newData = true; + } + } + + else if (rc == startMarker) { + recvInProgress = true; + } + } +} + +//================================================================= RECEIVE 24 DATAS FROM PROCESSING + +void parseData() { // split the data into its parts + + char * strtokIndx; // this is used by strtok() as an index + + strtokIndx = strtok(tempChars,","); // get the first part - the string + /* + integerFromPC0 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC1 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC2 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC3 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC4 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC5 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC6 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC7 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC8 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + integerFromPC9 = atoi(strtokIndx); // convert this part to an integer +*/ + + // strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC0 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC1 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC2 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC3 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC4 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC5 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC6 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC7 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC8 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PC9 = atoi(strtokIndx); // convert this part to an integer + + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer0 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer1 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer2 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer3 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer4 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer5 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer6 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer7 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer8 = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + PCTer9 = atoi(strtokIndx); // convert this part to an integer + + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + orderTrigger = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + orderCohesion = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + orderCohesionB = atoi(strtokIndx); // convert this part to an integer + strtokIndx = strtok(NULL, ","); // this continues where the previous call left off + startStop = atoi(strtokIndx); // convert this part to an integer + +} + +//============ + + + void readDataFromProcessingToSerial1() { // READ 30 datas even if there is ony 24 datas from Processing + +// DONT FORGET SerialUSB.println at the end of each line!!!!! + Serial1.print ("A "); Serial1.println (integerFromPC0); + Serial1.print ("B "); Serial1.println (integerFromPC1); + Serial1.print ("C "); Serial1.println (integerFromPC2); + Serial1.print ("D "); Serial1.println (integerFromPC3); + Serial1.print ("E "); Serial1.println (integerFromPC4); + Serial1.print ("F "); Serial1.println (integerFromPC5); + Serial1.print ("G "); Serial1.println (integerFromPC6); + Serial1.print ("H "); Serial1.println (integerFromPC7); + Serial1.print ("I "); Serial1.println (integerFromPC8); + Serial1.print ("J "); Serial1.println (integerFromPC9); + + Serial1.print ("K ");Serial1.println(PC0); + Serial1.print ("L ");Serial1.println(PC1); + Serial1.print ("M ");Serial1.println(PC2); + Serial1.print ("N ");Serial1.println(PC3); + Serial1.print ("O ");Serial1.println(PC4); + Serial1.print ("P ");Serial1.println(PC5); + Serial1.print ("Q ");Serial1.println(PC6); + Serial1.print ("R ");Serial1.println(PC7); + Serial1.print ("S ");Serial1.println(PC8); + Serial1.print ("T ");Serial1.println(PC9); + + Serial1.print ("U ");Serial1.println(positionX); + Serial1.print ("V ");Serial1.println(positionX1); + Serial1.print ("W ");Serial1.println(positionX2); + Serial1.print ("X ");Serial1.println(positionX3); + Serial1.print ("Y ");Serial1.println(positionX4); + Serial1.print ("Z ");Serial1.println(positionX5); + Serial1.print ("a ");Serial1.println(positionX6); + Serial1.print ("b ");Serial1.println(positionX7); + Serial1.print ("c ");Serial1.println(positionX8); + Serial1.print ("d ");Serial1.println(positionX9); + +} + +void noJoeTransformation() + { + + + stepper[5].moveTo(PC5); + stepper[5].run(); + + + stepper[4].moveTo(PC4); + stepper[4].run(); + stepper[3].moveTo(PC3); + stepper[3].run(); + stepper[2].moveTo(PC2); + stepper[2].run(); + stepper[1].moveTo(PC1); + stepper[1].run(); + + + stepper[0].moveTo(PC0); // premiere donnée envoyée dansla chaine de String de Processing = virtualPositon + stepper[0].run(); +} + + void sendPositionNativePort() { // + + +// DONT FORGET SerialUSB.println at the end of each line!!!!! +/* + SerialUSB.print ("A "); SerialUSB.println (integerFromPC0); + SerialUSB.print ("B "); SerialUSB.println (integerFromPC1); + SerialUSB.print ("C "); SerialUSB.println (integerFromPC2); + SerialUSB.print ("D "); SerialUSB.println (integerFromPC3); + SerialUSB.print ("E "); SerialUSB.println (integerFromPC4); + SerialUSB.print ("F "); SerialUSB.println (integerFromPC5); + SerialUSB.print ("G "); SerialUSB.println (integerFromPC6); + SerialUSB.print ("H "); SerialUSB.println (integerFromPC7); + SerialUSB.print ("I "); SerialUSB.println (integerFromPC8); + SerialUSB.print ("J "); SerialUSB.println (integerFromPC9); + + SerialUSB.print ("K ");SerialUSB.println(PC0); + SerialUSB.print ("L ");SerialUSB.println(PC1); + SerialUSB.print ("M ");SerialUSB.println(PC2); + SerialUSB.print ("N ");SerialUSB.println(PC3); + SerialUSB.print ("O ");SerialUSB.println(PC4); + SerialUSB.print ("P ");SerialUSB.println(PC5); + SerialUSB.print ("Q ");SerialUSB.println(PC6); + SerialUSB.print ("R ");SerialUSB.println(PC7); + SerialUSB.print ("S ");SerialUSB.println(PC8); + SerialUSB.print ("T ");SerialUSB.println(PC9); + + SerialUSB.print ("U ");SerialUSB.println(positionX); + SerialUSB.print ("V ");SerialUSB.println(positionX1); + SerialUSB.print ("W ");SerialUSB.println(positionX2); + SerialUSB.print ("X ");SerialUSB.println(positionX3); + SerialUSB.print ("Y ");SerialUSB.println(positionX4); + SerialUSB.print ("Z ");SerialUSB.println(positionX5); + SerialUSB.print ("a ");SerialUSB.println(positionX6); + SerialUSB.print ("b ");SerialUSB.println(positionX7); + SerialUSB.print ("c ");SerialUSB.println(positionX8); + SerialUSB.print ("d ");SerialUSB.println(positionX9); + */ +} + + + +void tickSteppers() +{ + + // Serial1.print ("AftertickSteppers>"); + // Serial1.println (micros()); +} diff --git a/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.cpp b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.cpp new file mode 100644 index 0000000..7e44d34 --- /dev/null +++ b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.cpp @@ -0,0 +1,163 @@ +#include "io.h" +#include "com.h" + +void initCom() { +// Serial.begin(115200); +// Serial1.begin(115200); +} + +void printSerialCommandsAvailable() { + Serial.print(""); + Serial.println("Liste des commandes disponibles VERSION DEV 1.0"); + Serial.println(""); + Serial.println("help= : Montre l'ensemble des commandes serial disponible"); + Serial.println("goal=N Steps : Donne l'objectif en step au moteur N"); + Serial.println("maxspeed=N Steps : Donne l'objectif en step.s-1 au moteur N"); + Serial.println("accel=N Steps : Donne l'objectif en step.s-2 au moteur N"); + Serial.println("go= : Fait bouger tous les moteurs"); + Serial.println("rotate= Fait tourner tous les moteurs à leur vitesse maxspeed"); + Serial.println("override=PERCENT change proportionnellement la vitesse de tous les moteurs en % de maxspeed"); + Serial.println("init= : Lance la séquence d'initialisation"); + Serial.println("codeur=N : Donne la position du codeur N"); + Serial.println("codeurs= : Donne la position de tous les codeurs"); + Serial.println("stop= : Stop les moteur "); + + Serial.println(""); +} + +bool readSerial() { +// bool received = false; +// while (Serial.available()) { +// char data = Serial.read(); +// addCharToSerialBuffer(data); +// received = true; +// } +// return received; +} + +// Read Serial data storage +char cmdOp[NBCHAR] = {""}; +char cmdArg[NBCHAR] = {""}; +char cmdArg2[NBCHAR] = {""}; + +/** + Lors de l'envoi d'une commande, enregistre la commande ainsi que son ou ses arguments + et lance la fonction parseSerialBuffer() pour traiter la commande +*/ +void addCharToSerialBuffer(char c) { + static uint8_t cmdState = 0; + static uint8_t cmdOpi = 0; + static uint16_t cmdArgi = 0; + static uint16_t cmdArg2i = 0; + + switch (cmdState) + { + case 0 : + if (c == '\n') + cmdOpi = 0; + else if (c == '=') { + cmdOp[cmdOpi] = '\0'; + cmdArgi = 0; + cmdState = 1; + } + else if (cmdOpi < sizeof(cmdOp) - 1) + cmdOp[cmdOpi++] = c; + break; + + case 1 : + if (c == '\n') { + cmdArg[cmdArgi] = 0; + cmdArg2[0] = '\0'; + parseSerialBuffer(); + cmdOpi = 0; + cmdState = 0; + } + else if (c == ' ') { + cmdArg[cmdArgi] = 0; + cmdOpi = 0; + cmdArg2i = 0; + cmdState = 2; + } + else if (cmdArgi < sizeof(cmdArg) - 1) + cmdArg[cmdArgi++] = c; + break; + case 2 : + if (c == '\n') { + cmdArg2[cmdArg2i] = '\0'; + parseSerialBuffer(); + cmdState = 0; + } + else if (cmdArg2i < sizeof(cmdArg2) - 1) + cmdArg2[cmdArg2i++] = c; + break; + } +} + +void parseSerialBuffer() { // Parse serial data, handle all commands available over Serial + + int32_t val = atol(cmdArg); + int32_t val2 = atol(cmdArg2); + + // help= : Montre l'ensemble des commandes serial disponible // USER ONLY + if (!strcmp(cmdOp, "help")) { + printSerialCommandsAvailable(); + } + else if (!strcmp(cmdOp, "go")) { + Serial.println("go"); + moveMotorsAsync(); + } + else if (!strcmp(cmdOp, "goal")) { + printCmdValVal("goal", val, val2); + setGoal(val, val2); + } + else if (!strcmp(cmdOp, "maxspeed")) { + printCmdValVal("maxspeed", val, val2); + setMaxspeed(val, val2); + } + else if (!strcmp(cmdOp, "accel")) { + printCmdValVal("accel", val, val2); + setAccel(val, val2); + } + else if (!strcmp(cmdOp, "init")) { + Serial.println("init"); + initSteppers(); + } + else if (!strcmp(cmdOp, "codeur")) { + printCmdValVal("codeur", val, getCount(val)); + } + else if (!strcmp(cmdOp, "codeurs")) { + for(uint8_t i=0; i < NBMOTORS; i++) + printCmdValVal("codeur", i, getCount(i)); + } + else if (!strcmp(cmdOp, "rotate")) { + Serial.println("rotate"); + rotateMotors(); + } + else if (!strcmp(cmdOp, "override")) { + printCmdVal("override", val); + overrideSpeed(val); + } + else if (!strcmp(cmdOp, "stop")) { + Serial.println("stop"); + stopMotors(); + } else { + Serial.println("Unknown => stop"); + stopMotors(); + } +} + +void printCmdVal( String cmd, int32_t val) { + Serial.print(cmd); + Serial.print("="); + Serial.print(val); + Serial.print("\n"); +} + +void printCmdValVal( String cmd, int32_t val1, int32_t val2) { + Serial.print(cmd); + Serial.print("="); + Serial.print(val1); + Serial.print(" "); + Serial.print(val2); + Serial.print("\n"); +} diff --git a/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.h b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.h new file mode 100644 index 0000000..1cfb6d7 --- /dev/null +++ b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/com.h @@ -0,0 +1,14 @@ +#ifndef com_h +#define com_h + +#define NBCHAR 20 // char size for text + +void initCom(); +void printSerialCommandsAvailable(); +bool readSerial(); +void addCharToSerialBuffer(char c); +void parseSerialBuffer(); +void printCmdVal( String cmd, int32_t val); +void printCmdValVal( String cmd, int32_t val1, int32_t val2); + +#endif diff --git a/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.cpp b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.cpp new file mode 100644 index 0000000..1ba2b85 --- /dev/null +++ b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.cpp @@ -0,0 +1,247 @@ +#include "io.h" +#include + +#define NBSTEPSPERTURN 6400 +#define MAXSPEED 6400 // Max 300 000 +#define ACCEL 40000 // Max 500 000 + +Stepper stepperMotors[NBMOTORS] = { Stepper(STEP[0], DIR[0]), Stepper(STEP[1], DIR[1]), Stepper(STEP[2], DIR[2]), Stepper(STEP[3], DIR[3]), Stepper(STEP[4], DIR[4]), Stepper(STEP[5], DIR[5]) }; +StepControl controller; + + +RotateControl rotateController(1, 500); + +bool isInit[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précisant si le moteur a été initialisé +int32_t goal[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +uint32_t maxspeed[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +uint32_t accel[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +volatile int32_t zeroPos[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Position zéro enregistrée quand Z passe de 1 à 0 +//const int32_t offsets[NBMOTORS] = {1300, 3720, 5120, 4720, 4370, 5920}; // => Boitier 1 ( étiquette) +const int32_t offsets[NBMOTORS] = {3990, 1030, 585, 1430, 750, 2050}; // => Boitier 2 + +void initSteppersPins() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + pinMode(EN[i], OUTPUT); + digitalWrite(EN[i], LOW); + pinMode(DIR[i], OUTPUT); + digitalWrite(DIR[i], LOW); + pinMode(STEP[i], OUTPUT); + digitalWrite(STEP[i], LOW); + setMaxspeed(i, MAXSPEED); + setAccel(i, ACCEL); + } +} + +void initSteppers() { + rotateController.stop(); + for(uint8_t i=0; i < NBMOTORS; i++) { + setMaxspeed(i, MAXSPEED); + setStep(i,0); + setGoal(i, NBSTEPSPERTURN); + } +// moveMotors(); + for(uint8_t i=0; i < NBMOTORS; i++) { + int32_t val =(zeroPos[i] + offsets[i]) % NBSTEPSPERTURN; + if(val < NBSTEPSPERTURN / 2) + val += NBSTEPSPERTURN; + setGoal(i,val); + } + // moveMotors(); + for(uint8_t i=0; i < NBMOTORS; i++) { + setStep(i,0); + setGoal(i,0); + setCount(i,0); + } + Serial.println("initDone"); +} + +void setStep(uint8_t motorNumber, int32_t val) { + stepperMotors[motorNumber].setPosition(val); // TeensyStep + //stepperMotors[motorNumber].setCurrentPosition(val); // accelStepper +} + +void setGoal(uint8_t motorNumber, int32_t val) { + goal[motorNumber] = val; +} + +void setMaxspeed(uint8_t motorNumber, int32_t val) { + maxspeed[motorNumber] = val; + stepperMotors[motorNumber].setMaxSpeed(maxspeed[motorNumber]); // steps/s +} + +void setAccel(uint8_t motorNumber, int32_t val) { + accel[motorNumber] = val; // steps/s^2 + stepperMotors[motorNumber].setAcceleration(accel[motorNumber]); +} + +void moveMotors() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setTargetAbs(goal[i]); + //stepperMotors[i].moveTo(goal[i]); // AccelStepper + } + controller.move(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); + //controller.move(stepperMotors); + +} + +void moveMotorsAsync() { + rotateController.stop(); + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setTargetAbs(goal[i]); + //stepperMotors[i].moveTo(goal[i]); // AccelStepper + } + //controller.moveAsync(stepperMotors); + controller.moveAsync(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); +} + + +void rotateMotors() { + controller.stop(); + rotateController.stop(); + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setMaxSpeed(maxspeed[i]); + } + rotateController.rotateAsync(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); +} + +void overrideSpeed(int percent) { + float ratio = percent / 100.0; + rotateController.overrideSpeed(ratio); +} + +void stopMotors(){ + overrideSpeed(0); + rotateController.stop(); + controller.stop(); +} + +int32_t getStep(uint8_t motorNumber) { + return stepperMotors[motorNumber].getPosition(); // TeensyStep + // return stepperMotors[motorNumber].currentPosition(); // accelStepper +} + +// Encodeurs +volatile int32_t count[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Comptage des tics d'encodeur qui est incrémenté à chaque interruption +volatile int16_t rotationSpeed[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Vitesse de rotation extraite de la variable count +volatile bool laststateA[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties A des enncodeurs +volatile bool laststateB[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties B des enncodeurs +volatile bool laststateZ[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties Z des enncodeurs + + +void counterA(uint8_t motorNumber) { + bool stateA = digitalRead(ENCODERAS[motorNumber]); + (stateA ^ laststateB[motorNumber]) ? count[motorNumber]++ : count[motorNumber]--; + laststateA[motorNumber] = stateA; +} + +void counterB(uint8_t motorNumber) { + bool stateB = digitalRead(ENCODERBS[motorNumber]); + (stateB ^ laststateA[motorNumber]) ? count[motorNumber]-- : count[motorNumber]++; + laststateB[motorNumber] = stateB; +} + +void interruptZ(uint8_t motorNumber) { + bool stateZ = digitalRead(ENCODERZS[motorNumber]); + zeroPos[motorNumber] = getStep(motorNumber); + laststateZ[motorNumber] = stateZ; +} + +void counterA0() { + counterA(0); +} +void counterA1() { + counterA(1); +} +void counterA2() { + counterA(2); +} +void counterA3() { + counterA(3); +} +void counterA4() { + counterA(4); +} +void counterA5() { + counterA(5); +} +void counterB0() { + counterB(0); +} +void counterB1() { + counterB(1); +} +void counterB2() { + counterB(2); +} +void counterB3() { + counterB(3); +} +void counterB4() { + counterB(4); +} +void counterB5() { + counterB(5); +} +void interruptZ0() { + interruptZ(0); +} +void interruptZ1() { + interruptZ(1); +} +void interruptZ2() { + interruptZ(2); +} +void interruptZ3() { + interruptZ(3); +} +void interruptZ4() { + interruptZ(4); +} +void interruptZ5() { + interruptZ(5); +} + +void (*counterEncoderA[])(void) = { counterA0, counterA1, counterA2, counterA3, counterA4, counterA5 }; +void (*counterEncoderB[])(void) = { counterB0, counterB1, counterB2, counterB3, counterB4, counterB5 }; +void (*interruptEncoderZ[])(void) = { interruptZ0, interruptZ1, interruptZ2, interruptZ3, interruptZ4, interruptZ5 }; + +void initEncoders() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + pinMode(ENCODERAS[i], INPUT_PULLUP); + pinMode(ENCODERBS[i], INPUT_PULLUP); + pinMode(ENCODERZS[i], INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(ENCODERAS[i]), counterEncoderA[i], CHANGE); //S'active lorsqu'il y a un changement d'état sur la pin de l'encodeur A + attachInterrupt(digitalPinToInterrupt(ENCODERBS[i]), counterEncoderB[i], CHANGE); //S'active lorsqu'il y a un changement d'état sur la pin de l'encodeur B + attachInterrupt(digitalPinToInterrupt(ENCODERZS[i]), interruptEncoderZ[i], FALLING); //S'active lorsqu'il y a un changement d'état haut vers bas sur la pin de l'encodeur Z + } +} + +int32_t getCount(uint8_t motorId) { + return count[motorId]; +} + +void setCount(uint8_t motorId, int32_t val) { + count[motorId] = val; +} + +void toggleLed() { + static bool ledState = LOW; + ledState = !ledState; + digitalWrite(LED, ledState); +} + +void initIo() { + pinMode(LED, OUTPUT); + initSteppersPins(); + initEncoders(); +} + +void displayAllCountSerial1() { + Serial1.print("<"); + for(uint8_t i=0; i < NBMOTORS - 1; i++) { + Serial1.print(getCount(i)); + Serial1.print(","); + } + Serial1.print(getCount(NBMOTORS - 1)); + Serial1.println(">"); +} \ No newline at end of file diff --git a/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.h b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.h new file mode 100644 index 0000000..81677d2 --- /dev/null +++ b/Position6400DUE24dataEnableNoJoeDriverAffiche_PositionMoteur/io.h @@ -0,0 +1,42 @@ +#ifndef io_h +#define io_h + +#include "Arduino.h" + +void initIo(); + +// Simple IO +#define LED 13 +void toggleLed(); + +// Motors +#define NBMOTORS 6 + +const uint8_t STEP[NBMOTORS] = {5, 8, 11, 25, 28, 31}; +const uint8_t DIR[NBMOTORS] = {6, 9, 12, 26, 29, 32}; +const uint8_t EN[NBMOTORS] = {4, 7, 10, 24, 27, 30}; +#define STEPMAX 6400 +void initSteppersPins(); +void initSteppers(); +int32_t getStep(uint8_t motorId); +void setStep(uint8_t motorId, int32_t val); +void setGoal(uint8_t motorNumber, int32_t val); +void setMaxspeed(uint8_t motorNumber, int32_t val); +void setAccel(uint8_t motorNumber, int32_t val); +void moveMotors(); +void moveMotorsAsync(); +void rotateMotors(); +void overrideSpeed(int percent); +void stopMotors(); + +// Encoders +const uint8_t ENCODERAS[NBMOTORS] = {21, 14, 18, 15, 36, 33}; +const uint8_t ENCODERBS[NBMOTORS] = {22, 2, 19, 16, 37, 34}; +const uint8_t ENCODERZS[NBMOTORS] = {23, 39, 20, 17, 38, 35}; +#define ENCODERMAX 4000 +void initEncoders(); +int32_t getCount(uint8_t motorId); +void setCount(uint8_t motorId, int32_t val); +void displayAllCountSerial1(); + +#endif diff --git a/originaccelstepprocessing/originaccelstepprocessing.ino b/RecevoirDonn_esProcessing_EncodeurEtControleMoteurs.ino similarity index 100% rename from originaccelstepprocessing/originaccelstepprocessing.ino rename to RecevoirDonn_esProcessing_EncodeurEtControleMoteurs.ino diff --git a/teensystepdemo/com.cpp b/teensystepdemo/com.cpp new file mode 100644 index 0000000..7443d74 --- /dev/null +++ b/teensystepdemo/com.cpp @@ -0,0 +1,163 @@ +#include "io.h" +#include "com.h" + +void initCom() { + Serial.begin(115200); + Serial1.begin(115200); +} + +void printSerialCommandsAvailable() { + Serial.print(""); + Serial.println("Liste des commandes disponibles VERSION DEV 1.0"); + Serial.println(""); + Serial.println("help= : Montre l'ensemble des commandes serial disponible"); + Serial.println("goal=N Steps : Donne l'objectif en step au moteur N"); + Serial.println("maxspeed=N Steps : Donne l'objectif en step.s-1 au moteur N"); + Serial.println("accel=N Steps : Donne l'objectif en step.s-2 au moteur N"); + Serial.println("go= : Fait bouger tous les moteurs"); + Serial.println("rotate= Fait tourner tous les moteurs à leur vitesse maxspeed"); + Serial.println("override=PERCENT change proportionnellement la vitesse de tous les moteurs en % de maxspeed"); + Serial.println("init= : Lance la séquence d'initialisation"); + Serial.println("codeur=N : Donne la position du codeur N"); + Serial.println("codeurs= : Donne la position de tous les codeurs"); + Serial.println("stop= : Stop les moteur "); + + Serial.println(""); +} + +bool readSerial() { + bool received = false; + while (Serial.available()) { + char data = Serial.read(); + addCharToSerialBuffer(data); + received = true; + } + return received; +} + +// Read Serial data storage +char cmdOp[NBCHAR] = {""}; +char cmdArg[NBCHAR] = {""}; +char cmdArg2[NBCHAR] = {""}; + +/** + Lors de l'envoi d'une commande, enregistre la commande ainsi que son ou ses arguments + et lance la fonction parseSerialBuffer() pour traiter la commande +*/ +void addCharToSerialBuffer(char c) { + static uint8_t cmdState = 0; + static uint8_t cmdOpi = 0; + static uint16_t cmdArgi = 0; + static uint16_t cmdArg2i = 0; + + switch (cmdState) + { + case 0 : + if (c == '\n') + cmdOpi = 0; + else if (c == '=') { + cmdOp[cmdOpi] = '\0'; + cmdArgi = 0; + cmdState = 1; + } + else if (cmdOpi < sizeof(cmdOp) - 1) + cmdOp[cmdOpi++] = c; + break; + + case 1 : + if (c == '\n') { + cmdArg[cmdArgi] = 0; + cmdArg2[0] = '\0'; + parseSerialBuffer(); + cmdOpi = 0; + cmdState = 0; + } + else if (c == ' ') { + cmdArg[cmdArgi] = 0; + cmdOpi = 0; + cmdArg2i = 0; + cmdState = 2; + } + else if (cmdArgi < sizeof(cmdArg) - 1) + cmdArg[cmdArgi++] = c; + break; + case 2 : + if (c == '\n') { + cmdArg2[cmdArg2i] = '\0'; + parseSerialBuffer(); + cmdState = 0; + } + else if (cmdArg2i < sizeof(cmdArg2) - 1) + cmdArg2[cmdArg2i++] = c; + break; + } +} + +void parseSerialBuffer() { // Parse serial data, handle all commands available over Serial + + int32_t val = atol(cmdArg); + int32_t val2 = atol(cmdArg2); + + // help= : Montre l'ensemble des commandes serial disponible // USER ONLY + if (!strcmp(cmdOp, "help")) { + printSerialCommandsAvailable(); + } + else if (!strcmp(cmdOp, "go")) { + Serial.println("go"); + moveMotorsAsync(); + } + else if (!strcmp(cmdOp, "goal")) { + printCmdValVal("goal", val, val2); + setGoal(val, val2); + } + else if (!strcmp(cmdOp, "maxspeed")) { + printCmdValVal("maxspeed", val, val2); + setMaxspeed(val, val2); + } + else if (!strcmp(cmdOp, "accel")) { + printCmdValVal("accel", val, val2); + setAccel(val, val2); + } + else if (!strcmp(cmdOp, "init")) { + Serial.println("init"); + initSteppers(); + } + else if (!strcmp(cmdOp, "codeur")) { + printCmdValVal("codeur", val, getCount(val)); + } + else if (!strcmp(cmdOp, "codeurs")) { + for(uint8_t i=0; i < NBMOTORS; i++) + printCmdValVal("codeur", i, getCount(i)); + } + else if (!strcmp(cmdOp, "rotate")) { + Serial.println("rotate"); + rotateMotors(); + } + else if (!strcmp(cmdOp, "override")) { + printCmdVal("override", val); + overrideSpeed(val); + } + else if (!strcmp(cmdOp, "stop")) { + Serial.println("stop"); + stopMotors(); + } else { + Serial.println("Unknown => stop"); + stopMotors(); + } +} + +void printCmdVal( String cmd, int32_t val) { + Serial.print(cmd); + Serial.print("="); + Serial.print(val); + Serial.print("\n"); +} + +void printCmdValVal( String cmd, int32_t val1, int32_t val2) { + Serial.print(cmd); + Serial.print("="); + Serial.print(val1); + Serial.print(" "); + Serial.print(val2); + Serial.print("\n"); +} diff --git a/teensystepdemo/com.h b/teensystepdemo/com.h new file mode 100644 index 0000000..1cfb6d7 --- /dev/null +++ b/teensystepdemo/com.h @@ -0,0 +1,14 @@ +#ifndef com_h +#define com_h + +#define NBCHAR 20 // char size for text + +void initCom(); +void printSerialCommandsAvailable(); +bool readSerial(); +void addCharToSerialBuffer(char c); +void parseSerialBuffer(); +void printCmdVal( String cmd, int32_t val); +void printCmdValVal( String cmd, int32_t val1, int32_t val2); + +#endif diff --git a/teensystepdemo/io.cpp b/teensystepdemo/io.cpp new file mode 100644 index 0000000..11b1ed3 --- /dev/null +++ b/teensystepdemo/io.cpp @@ -0,0 +1,248 @@ +#include "io.h" +#include + +void initIo() { + pinMode(LED, OUTPUT); + initSteppersPins(); + initEncoders(); +} + +void toggleLed() { + static bool ledState = LOW; + ledState = !ledState; + digitalWrite(LED, ledState); +} + +// STEPPER + +#define NBSTEPSPERTURN 6400 +#define MAXSPEED 6400 // Max 300 000 +#define ACCEL 40000 // Max 500 000 + +Stepper stepperMotors[NBMOTORS] = { Stepper(STEP[0], DIR[0]), Stepper(STEP[1], DIR[1]), Stepper(STEP[2], DIR[2]), Stepper(STEP[3], DIR[3]), Stepper(STEP[4], DIR[4]), Stepper(STEP[5], DIR[5]) }; +StepControl controller; + + +RotateControl rotateController(1, 500); + +bool isInit[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précisant si le moteur a été initialisé +int32_t goal[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +uint32_t maxspeed[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +uint32_t accel[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; +volatile int32_t zeroPos[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Position zéro enregistrée quand Z passe de 1 à 0 +//const int32_t offsets[NBMOTORS] = {1300, 3720, 5120, 4720, 4370, 5920}; // => Boitier 1 ( étiquette) +const int32_t offsets[NBMOTORS] = {3990, 1030, 585, 1430, 750, 2050}; // => Boitier 2 + +void initSteppersPins() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + pinMode(EN[i], OUTPUT); + digitalWrite(EN[i], LOW); + pinMode(DIR[i], OUTPUT); + digitalWrite(DIR[i], LOW); + pinMode(STEP[i], OUTPUT); + digitalWrite(STEP[i], LOW); + setMaxspeed(i, MAXSPEED); + setAccel(i, ACCEL); + } +} + +void initSteppers() { + rotateController.stop(); + for(uint8_t i=0; i < NBMOTORS; i++) { + setMaxspeed(i, MAXSPEED); + setStep(i,0); + setGoal(i, NBSTEPSPERTURN); + } + moveMotors(); + for(uint8_t i=0; i < NBMOTORS; i++) { + int32_t val =(zeroPos[i] + offsets[i]) % NBSTEPSPERTURN; + if(val < NBSTEPSPERTURN / 2) + val += NBSTEPSPERTURN; + setGoal(i,val); + } + moveMotors(); + for(uint8_t i=0; i < NBMOTORS; i++) { + setStep(i,0); + setGoal(i,0); + setCount(i,0); + } + Serial.println("initDone"); +} + +void setStep(uint8_t motorNumber, int32_t val) { + stepperMotors[motorNumber].setPosition(val); // TeensyStep + //stepperMotors[motorNumber].setCurrentPosition(val); // accelStepper +} + +void setGoal(uint8_t motorNumber, int32_t val) { + goal[motorNumber] = val; +} + +void setMaxspeed(uint8_t motorNumber, int32_t val) { + maxspeed[motorNumber] = val; + stepperMotors[motorNumber].setMaxSpeed(maxspeed[motorNumber]); // steps/s +} + +void setAccel(uint8_t motorNumber, int32_t val) { + accel[motorNumber] = val; // steps/s^2 + stepperMotors[motorNumber].setAcceleration(accel[motorNumber]); +} + +void moveMotors() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setTargetAbs(goal[i]); + //stepperMotors[i].moveTo(goal[i]); // AccelStepper + } + controller.move(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); +} + +void moveMotorsAsync() { + rotateController.stop(); + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setTargetAbs(goal[i]); + //stepperMotors[i].moveTo(goal[i]); // AccelStepper + } + controller.moveAsync(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); +} + + +void rotateMotors() { + controller.stop(); + rotateController.stop(); + for (uint8_t i = 0; i < NBMOTORS; i++) { + stepperMotors[i].setMaxSpeed(maxspeed[i]); + } + rotateController.rotateAsync(stepperMotors[0], stepperMotors[1], stepperMotors[2], stepperMotors[3], stepperMotors[4], stepperMotors[5]); +} + +void overrideSpeed(int percent) { + float ratio = percent / 100.0; + rotateController.overrideSpeed(ratio); +} + +void stopMotors(){ + overrideSpeed(0); + rotateController.stop(); + controller.stop(); +} + +int32_t getStep(uint8_t motorNumber) { + return stepperMotors[motorNumber].getPosition(); // TeensyStep + // return stepperMotors[motorNumber].currentPosition(); // accelStepper +} + + +// Encodeurs + +volatile int32_t count[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Comptage des tics d'encodeur qui est incrémenté à chaque interruption +volatile int16_t rotationSpeed[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Vitesse de rotation extraite de la variable count +volatile bool laststateA[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties A des enncodeurs +volatile bool laststateB[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties B des enncodeurs +volatile bool laststateZ[NBMOTORS] = { 0, 0, 0, 0, 0, 0 }; //Etat précédent des sorties Z des enncodeurs + + +void counterA(uint8_t motorNumber) { + bool stateA = digitalRead(ENCODERAS[motorNumber]); + (stateA ^ laststateB[motorNumber]) ? count[motorNumber]++ : count[motorNumber]--; + laststateA[motorNumber] = stateA; +} + +void counterB(uint8_t motorNumber) { + bool stateB = digitalRead(ENCODERBS[motorNumber]); + (stateB ^ laststateA[motorNumber]) ? count[motorNumber]-- : count[motorNumber]++; + laststateB[motorNumber] = stateB; +} + +void interruptZ(uint8_t motorNumber) { + bool stateZ = digitalRead(ENCODERZS[motorNumber]); + zeroPos[motorNumber] = getStep(motorNumber); + laststateZ[motorNumber] = stateZ; +} + +void counterA0() { + counterA(0); +} +void counterA1() { + counterA(1); +} +void counterA2() { + counterA(2); +} +void counterA3() { + counterA(3); +} +void counterA4() { + counterA(4); +} +void counterA5() { + counterA(5); +} +void counterB0() { + counterB(0); +} +void counterB1() { + counterB(1); +} +void counterB2() { + counterB(2); +} +void counterB3() { + counterB(3); +} +void counterB4() { + counterB(4); +} +void counterB5() { + counterB(5); +} +void interruptZ0() { + interruptZ(0); +} +void interruptZ1() { + interruptZ(1); +} +void interruptZ2() { + interruptZ(2); +} +void interruptZ3() { + interruptZ(3); +} +void interruptZ4() { + interruptZ(4); +} +void interruptZ5() { + interruptZ(5); +} + +void (*counterEncoderA[])(void) = { counterA0, counterA1, counterA2, counterA3, counterA4, counterA5 }; +void (*counterEncoderB[])(void) = { counterB0, counterB1, counterB2, counterB3, counterB4, counterB5 }; +void (*interruptEncoderZ[])(void) = { interruptZ0, interruptZ1, interruptZ2, interruptZ3, interruptZ4, interruptZ5 }; + +void initEncoders() { + for (uint8_t i = 0; i < NBMOTORS; i++) { + pinMode(ENCODERAS[i], INPUT_PULLUP); + pinMode(ENCODERBS[i], INPUT_PULLUP); + pinMode(ENCODERZS[i], INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(ENCODERAS[i]), counterEncoderA[i], CHANGE); //S'active lorsqu'il y a un changement d'état sur la pin de l'encodeur A + attachInterrupt(digitalPinToInterrupt(ENCODERBS[i]), counterEncoderB[i], CHANGE); //S'active lorsqu'il y a un changement d'état sur la pin de l'encodeur B + attachInterrupt(digitalPinToInterrupt(ENCODERZS[i]), interruptEncoderZ[i], FALLING); //S'active lorsqu'il y a un changement d'état haut vers bas sur la pin de l'encodeur Z + } +} + +int32_t getCount(uint8_t motorId) { + return count[motorId]; +} + +void setCount(uint8_t motorId, int32_t val) { + count[motorId] = val; +} + +void displayAllCountSerial1() { + Serial1.print("<"); + for(uint8_t i=0; i < NBMOTORS - 1; i++) { + Serial1.print(getCount(i)); + Serial1.print(","); + } + Serial1.print(getCount(NBMOTORS - 1)); + Serial1.println(">"); +} diff --git a/teensystepdemo/io.h b/teensystepdemo/io.h new file mode 100644 index 0000000..81677d2 --- /dev/null +++ b/teensystepdemo/io.h @@ -0,0 +1,42 @@ +#ifndef io_h +#define io_h + +#include "Arduino.h" + +void initIo(); + +// Simple IO +#define LED 13 +void toggleLed(); + +// Motors +#define NBMOTORS 6 + +const uint8_t STEP[NBMOTORS] = {5, 8, 11, 25, 28, 31}; +const uint8_t DIR[NBMOTORS] = {6, 9, 12, 26, 29, 32}; +const uint8_t EN[NBMOTORS] = {4, 7, 10, 24, 27, 30}; +#define STEPMAX 6400 +void initSteppersPins(); +void initSteppers(); +int32_t getStep(uint8_t motorId); +void setStep(uint8_t motorId, int32_t val); +void setGoal(uint8_t motorNumber, int32_t val); +void setMaxspeed(uint8_t motorNumber, int32_t val); +void setAccel(uint8_t motorNumber, int32_t val); +void moveMotors(); +void moveMotorsAsync(); +void rotateMotors(); +void overrideSpeed(int percent); +void stopMotors(); + +// Encoders +const uint8_t ENCODERAS[NBMOTORS] = {21, 14, 18, 15, 36, 33}; +const uint8_t ENCODERBS[NBMOTORS] = {22, 2, 19, 16, 37, 34}; +const uint8_t ENCODERZS[NBMOTORS] = {23, 39, 20, 17, 38, 35}; +#define ENCODERMAX 4000 +void initEncoders(); +int32_t getCount(uint8_t motorId); +void setCount(uint8_t motorId, int32_t val); +void displayAllCountSerial1(); + +#endif diff --git a/teensystepdemo/teensystepdemo.ino b/teensystepdemo/teensystepdemo.ino new file mode 100644 index 0000000..e92bd57 --- /dev/null +++ b/teensystepdemo/teensystepdemo.ino @@ -0,0 +1,20 @@ +#include "io.h" +#include "com.h" +#include // Include the Metro library + +Metro newTest = Metro(100); // Configure test flag to 100ms interval + +void setup() { + initIo(); + initCom(); + initSteppers(); + printSerialCommandsAvailable(); +} + +void loop() { + readSerial(); + if (newTest.check()) { + toggleLed(); + displayAllCountSerial1(); + } +}