Code:
#include <AccelStepper.h> // Define a stepper and the pins it will use
#define NBMOTEURS 10
#define NBPASPARTOUR 3200 // Set on the driver
#define STEP 1 --> My motor at full step needs 200 steps to make one revolution
Mode 1/4 pas MS2 sur ON --> 800 step/round
Mode 1/8 MS1+ MS2 sur ON --> 1600 step/round
Mode 1/16 MS3 sur ON --> 3200
Mode 1/32 MS1+ MS3 sur ON --> 6400
//******* From the behind to the front
// 1600 Arduino Due
// const uint8_t PINDIRECTION[NBMOTEURS] = { 11, 7, 3, 23, 33, 37, 41, 45, 49, 53 };//{ 10, 6, 2, 22, 26};
// const uint8_t PINSPEED[NBMOTEURS]= { 10, 6, 2, 22, 32, 36, 40, 44, 48, 52 }; //{ 11, 7, 3, 23, 27};
// 3200 Teensy
const uint8_t PINDIRECTION[NBMOTEURS] = { 2, 4, 6, 8, 10, 45, 49, 53, 12, 99 };//{ 10, 6, 2, 22, 26};
const uint8_t PINSPEED[NBMOTEURS]= { 3, 5, 7, 9, 11, 44, 48, 52, 13, 98 }; //{ 11, 7, 3, 23, 27};
// 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]),
AccelStepper (STEP, PINSPEED[6], PINDIRECTION[6]),
AccelStepper (STEP, PINSPEED[7], PINDIRECTION[7]),
AccelStepper (STEP, PINSPEED[8], PINDIRECTION[8]),
AccelStepper (STEP, PINSPEED[9], PINDIRECTION[9]),
};
// 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 orderTrigger = 0;
int orderCohesion = 0;
int orderCohesionB = 0;
float floatFromPC = 0.0; // not used for the moment
boolean newData = false;
//============
IntervalTimer t1;
#define TX_SIZE 512
uint8_t tx_buffer[TX_SIZE];
void setup()
{
// Serial1.begin(115200); // lot of noise and rumble without changes buffer size in serial1.c 64 before, now 255
Serial1.begin(115200); // work perfectly now.
Serial .begin(115200); // work better
// Serial1.addMemoryForWrite(tx_buffer,TX_SIZE); Don't work
NVIC_SET_PRIORITY(IRQ_UART0_STATUS, 16); //32
t1.priority(16); //32
t1.begin(tickSteppers, 1000 ); // call every 1000µs, change if requred // make some noise
//====TEST of t1.begin(tickSteppers, 1000 ); Results below are without the 2 lines below
//NVIC_SET_PRIORITY(IRQ_UART0_STATUS, 32);
//t1.priority(32);
// 100: no difference,, motors shake at speed 40
// 1: block Processing and I don't the test in the setup
// 10: the test in the setup shake the motor
// 1000: motors shake at speed 70
// 2000: motors shake at speed 70
//====Test if datas come from the both serial of the Arduino Due or Teensy 3.6
Serial1.print ("A "); Serial1.println (-4);
Serial1.print ("B "); Serial1.println (-3);
Serial1.print ("C "); Serial1.println (-2);
Serial1.print ("D "); Serial1.println (-1);
Serial1.print ("E "); Serial1.println (-10);
Serial.print ("A "); Serial.println (4);
Serial.print ("B "); Serial.println (3);
Serial.print ("C "); Serial.println (2);
Serial.print ("D "); Serial.println (1);
Serial.print ("E "); Serial.println (10);
//====Initialise Pin Motor
for(uint8_t i = 0; i < NBMOTEURS; i++) {
pinMode(PINDIRECTION[i], OUTPUT);
digitalWrite(PINDIRECTION[i], OUTPUT);
pinMode(PINSPEED[i], OUTPUT);
digitalWrite(PINSPEED[i], OUTPUT);
/// with 1/16 step == 16*200= 3200 step / round
stepper[i].setMaxSpeed(12800); // WORK at 4 round/s
// stepper[i].setMaxSpeed(9000 ); //
stepper[i].setAcceleration(25600); // 4 tour/s-2 // WORK
// stepper[i].setAcceleration(9000);
}
PC0=25600; //8 tours
PC1=PC2=PC2=PC4=PC5=PC6=PC7=PC8=PC9= PC0;
stepper[9].moveTo(PC0);
stepper[8].moveTo(PC1);
stepper[7].moveTo(PC2);
stepper[6].moveTo(PC3);
stepper[5].moveTo(PC4);
stepper[4].moveTo(PC5);
stepper[3].moveTo(PC6);
stepper[2].moveTo(PC7);
stepper[1].moveTo(PC8);
stepper[0].moveTo(PC9);
}
void loop() {
recvWithStartEndMarkers(); //receive 70 datas*30 in on secondes
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(); // 33 datas marked and separated with a coma.
stepper[9].moveTo(PC0); stepper[8].moveTo(PC1); stepper[7].moveTo(PC2); stepper[6].moveTo(PC3); stepper[5].moveTo(PC4);
stepper[4].moveTo(PC5); stepper[3].moveTo(PC6); stepper[2].moveTo(PC7); stepper[1].moveTo(PC8); stepper[0].moveTo(PC9);
showPosition();
showPhazisScaled();
showTrigBangWithRevolution();
newData = false;
}
tickSteppers();
// stepper[9].run(); stepper[8].run(); stepper[7].run(); stepper[6].run(); stepper[5].run();
// stepper[4].run(); stepper[3].run(); stepper[2].run(); stepper[1].run(); stepper[0].run();
}
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
// if ( Serial.available() > 67 || recvInProgress == true ) // 20 or 2 or 60 no difference
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;
}
}
}
//============
void parseData() { // split the 31 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
}
//============
void showPosition() {
Serial1 .print ("K ");Serial1 .println(PC0);
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);
}
void showPhazisScaled() {
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);
}
void showTrigBangWithRevolution() {
// check to see if we have room enough in output queue to hold this...
// If (Serial1.availableForWrite() < MIN_SIZE_TO_ENABLE_WRITE) return;
Serial1.print ("U ");Serial1.println(PCTer0);
Serial1.print ("V ");Serial1.println(PCTer1);
Serial1.print ("W ");Serial1.println(PCTer2);
Serial1.print ("X ");Serial1.println(PCTer3);
Serial1.print ("Y ");Serial1.println(PCTer4);
Serial1.print ("Z ");Serial1.println(PCTer5);
Serial1.print ("a ");Serial1.println(PCTer6);
Serial1.print ("b ");Serial1.println(PCTer7);
Serial1.print ("c ");Serial1.println(PCTer8);
Serial1.print ("d ");Serial1.println(PCTer9);
Serial1.print ("e ");Serial1.println(orderTrigger);
Serial1.print ("f ");Serial1.println(orderCohesion);
Serial1.print ("g ");Serial1.println(orderCohesionB);
}
void tickSteppers()
{
stepper[0].run();
stepper[1].run();
stepper[2].run();
stepper[3].run();
stepper[4].run();
stepper[5].run();
stepper[6].run();
stepper[7].run();
stepper[8].run();
stepper[9].run();
}