I made it!!
#include <AccelStepper.h>
#include <SoftwareSerial.h>// import the serial library
#include "math.h"
SoftwareSerial Genotronex(10, 11); // RX, TX
int BluetoothData; // the data given from Computer
float Xm;
float Ym;
float W;
char junk = ' ';
AccelStepper stepper1(1, 3, 2); //MOTOR 1
int MS11 = 4;
int MS21 = 5;
int SLP1 = 6;
AccelStepper stepper3(1, 8, 7); // MOTOR 3
int MS13 = 9;
int MS23 = 12;
int SLP2 = 13;
AccelStepper stepper2(1, 23, 22); // MOTOR 2
int MS12 = 24;
int MS22 = 25;
int SLP3 = 26;
AccelStepper stepper4(1, 28, 27); // MOTOR 4
int MS14 = 29;
int MS24 = 30;
int SLP4 = 31;
//preračun hitrosti
float Pi= 3.1416;
int koraki = 200; //število korakov motorja za en obrat
float r = 0.029; //polmer kolesa
float l = 0.09; //širina
float d = 0.09; //dolžina
float obseg= 2*r*Pi; // obseg kolesa
void setup()
{
Genotronex.begin(115200); // begin serial output @ 115200 baud
Genotronex.println("For parameters input press: 1- Xm, 2- Ym and 3-W! ");
Genotronex.println("");
Genotronex.flush();
pinMode(2, OUTPUT); //MOTOR 1
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT); // MOTOR 3
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
pinMode(12, OUTPUT);
pinMode(13, OUTPUT);
pinMode(22, OUTPUT); // MOTOR 2
pinMode(23, OUTPUT);
pinMode(24, OUTPUT);
pinMode(25, OUTPUT);
pinMode(26, OUTPUT);
pinMode(27, OUTPUT); // MOTOR 4
pinMode(28, OUTPUT);
pinMode(29, OUTPUT);
pinMode(30, OUTPUT);
pinMode(31, OUTPUT);
digitalWrite(MS11, LOW); // MOTOR 1 MS1 MS2 (hitrost korakanja motorjev)
digitalWrite(MS21, LOW); // LOW LOW POLNOKORAČNO
digitalWrite(MS12, LOW); // MOTOR 2 HIGH LOW POLKORAČNO
digitalWrite(MS22, LOW); // LOW HIGH ČETRKORAČNO
digitalWrite(MS13, LOW); // MOTOR 3 HIGH HIGH OSMINSKOKORAČNO
digitalWrite(MS23, LOW);
digitalWrite(MS14, LOW); // MOTOR 4
digitalWrite(MS24, LOW);
int MAX= 700; // MAX HITROST (steps/s)
stepper1.setMaxSpeed(MAX);
stepper2.setMaxSpeed(MAX);
stepper3.setMaxSpeed(MAX);
stepper4.setMaxSpeed(MAX);
int pospesek= 50;
stepper1.setAcceleration(pospesek);
stepper2.setAcceleration(pospesek);
stepper4.setAcceleration(pospesek);
stepper4.setAcceleration(pospesek);
}
void loop()
{
Genotronex.println("For parameters input press: Xm, Ym and W! ");
Genotronex.println("Enter parameter value 'Xm', Press ENTER");
while (Genotronex.available() == 0) ;
{
Xm = Genotronex.parseFloat();
Genotronex.print("Xm = ");
Genotronex.println(Xm, DEC);
while (Genotronex.available() > 0) // .parseFloat() can leave non-numeric characters
{
junk = Genotronex.read() ; // clear the keyboard buffer
}
}
Genotronex.println("Enter parameter value 'Ym', Press ENTER");
while (Genotronex.available() == 0) ;
{
Ym = Genotronex.parseFloat();
Genotronex.print("Ym = ");
Genotronex.println(Ym, DEC);
while (Genotronex.available() > 0) // .parseFloat() can leave non-numeric characters
{
junk = Genotronex.read() ; // clear the keyboard buffer
}
}
Genotronex.println("Enter parameter value 'W', Press ENTER");
while (Genotronex.available() == 0) ;
{
W = Genotronex.parseFloat();
Genotronex.print("W = ");
Genotronex.println(W, DEC);
while (Genotronex.available() > 0) // .parseFloat() can leave non-numeric characters
{
junk = Genotronex.read() ; // clear the keyboard buffer
}
float v1= -0.7071*Xm + 0.7071*Ym + 0.1273*W;
float v2= -0.7071*Xm -0.7071*Ym + 0.1273*W;
float v3= 0.7071*Xm -0.7071*Ym + 0.1273*W;
float v4= 0.7071*Xm + 0.7071*Ym + 0.1273*W;
float obrati1 = v1 / obseg; //obrati prvega kolesa
float obrati2 = v2 / obseg; //obrati drugega kolesa
float obrati3 = v3 / obseg; //obrati tretjega kolesa
float obrati4 = v4 / obseg; //obrati četrtega kolesa
float stobrt1 = obrati1; //pretvorba float-->int
float stobrt2 = obrati2;
float stobrt3 = obrati3;
float stobrt4 = obrati4;
Genotronex.print("Obrati 1. kolesa = ");
Genotronex.println(obrati1, DEC);
Genotronex.println("\n");
Genotronex.println();
Genotronex.print("Obrati 2. kolesa = ");
Genotronex.println(obrati2, DEC);
Genotronex.println("\n");
Genotronex.println();
Genotronex.print("Obrati 3. kolesa = ");
Genotronex.println(obrati3, DEC);
Genotronex.println("\n");
Genotronex.println();
Genotronex.print("Obrati 4. kolesa = ");
Genotronex.println(obrati4, DEC);
Genotronex.println("\n");
Genotronex.println();
float SetMotorSpeed1 = stobrt1*koraki; // *200korakov na obrat/60sekund =korakov na sekundo
float SetMotorSpeed2 = stobrt2*koraki;
float SetMotorSpeed3 = stobrt3*koraki;
float SetMotorSpeed4 = stobrt4*koraki;
stepper1.setSpeed(SetMotorSpeed3); // hitrost posameznega kolesa
stepper2.setSpeed(SetMotorSpeed2);
stepper3.setSpeed(SetMotorSpeed1);
stepper4.setSpeed(SetMotorSpeed4);
stepper1.setSpeed(SetMotorSpeed1); // hitrost posameznega kolesa
stepper2.setSpeed(SetMotorSpeed2);
stepper3.setSpeed(SetMotorSpeed3);
stepper4.setSpeed(SetMotorSpeed4);
stepper1.runSpeed();
stepper2.runSpeed();
stepper3.runSpeed();
stepper4.runSpeed();
if( Xm && Ym && W ==0.0)
{
digitalWrite(SLP1, LOW);
digitalWrite(SLP2, LOW);
digitalWrite(SLP3, LOW);
digitalWrite(SLP4, LOW);
}
if( Xm || Ym || W !=0.0)
{
digitalWrite(SLP1, HIGH);
digitalWrite(SLP2, HIGH);
digitalWrite(SLP3, HIGH);
digitalWrite(SLP4, HIGH);
}
}
}
Now I just have one problem!
How to make this parameters to stay in buffer untill I rewrite parameters? Because I regulate speed of motors with this numbers.
Thanks!