



Argentina////////////////// Arduino controller V0.1 03/06/12 ///////////////
/// coded by : intructable ///
/// mofified with a lot of errors by : astro :( sorry ///
/// under public domain ///
////////////////////////////////////////////////////////////////////
#include <PID_v1.h>
// motor 1
#define M1PWM 5
#define M1A 4
#define M1B 3
// motor 2
#define M2PWM 10
#define M2A 9
#define M2B 8
//Motor constante
//PWM value when the motor sart to move
#define MINPOWER 110
//sensors
#define SENSOR1 4
#define SENSOR2 5
//sensors range
#define S1MIN 188
#define S1MAX 768
#define S2MIN 288
#define S2MAX 720
//Emergency button
#define EMERGENCYBUTTON 0
//Serial command
#define COMMAND_SIZE 128
char aWord[COMMAND_SIZE];
int nb_char=0;
int pot1=0;
int pot2=0;
char c;
//PID controller, actually a P controller (ei I =D =0)
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
double P=1;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
//Motor class
// function :
// Motor(int Pin_E, int Pin_MA, int Pin_MB) creator
// void setspeed(int mspeed) change the motor speed from -255 to 255
// void stopper() stop the motor
// void marche(bool Direction) change the motor direction
class Motor
{
public :
Motor(int Pin_E, int Pin_MA, int Pin_MB){
E= Pin_E;
MA =Pin_MA;
MB =Pin_MB;
pinMode(MA, OUTPUT);
digitalWrite(MA, LOW);
pinMode(MB, OUTPUT);
digitalWrite(MB, LOW);
pinMode(E, OUTPUT);
}
void setspeed(int mspeed)
{
stopper();
if(mspeed<0)
{
mspeed=-mspeed;
marche(false);
}
else
{
marche(true);
}
analogWrite(E,map(mspeed,0,255,MINPOWER,255));
}
void stopper()
{
digitalWrite(MA, LOW);
digitalWrite(MB, LOW);
}
void marche(bool Direction)
{
stopper();
if(Direction)digitalWrite(MA, HIGH);
else digitalWrite(MB, HIGH);
dir = Direction;
}
private :
int E,MA,MB;
bool dir;
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////
Motor motor1(M1PWM,M1A,M1B); //create a motor
Motor motor2(M2PWM,M2A,M2B); //create a motor
void setup()
{
Serial.begin(9600); //start serial comunication
Serial.print("START: "); //hello world
TCCR0B =TCCR0B &0b11111000 | 0x01; //change PWM frequency to avoid noise
attachInterrupt(EMERGENCYBUTTON, emergencyStop, CHANGE); //attach the emergency button to the emergency function
motor1.stopper(); //stop the motor 1
motor2.stopper(); //stop the motor 2
///////////CENTER//////////////////////
Setpoint1 =127; // set the setpoint to the medium value
Setpoint2 =127; // set the setpoint to the medium value
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
void loop()
{
serialread(); //read command, it understands command in the format "A00 X~a01~ Y~a02~~13~" with ~a01~ a decimal value from 0 to 255
Input1 = analogRead(SENSOR1); // read the potentiometer value
Input2 = analogRead(SENSOR2); // read the potentiometer value
pot1 = Input1; // read the potentiometer value
pot2 = Input2; // read the potentiometer value
Input1 = map(Input1,S1MIN,S1MAX,0,255); // convert the value in the range of the potentiometer, 0=min amplitude, 255=max amplitude
Input2 = map(Input2,S2MIN,S2MAX,0,255); // convert the value in the range of the potentiometer, 0=min amplitude, 255=max amplitude
Output1 = P*(Setpoint1-Input1); //regulate with a proportional controller
Output2 = P*(Setpoint2-Input2); //regulate with a proportional controller
motor1.setspeed(Output1); //set the motor speed
motor2.setspeed(Output2); //set the motor speed
// Serial.println(Input);
Serial.print("pot1=");
Serial.print(pot1);
Serial.print(" | MapInput1=");
Serial.print(Input1);
Serial.print(" | Target1=");
Serial.print(Setpoint1);
Serial.print(" | Move=");
Serial.print(Output1);
Serial.print("\t | \t pot2=");
Serial.print(pot2);
Serial.print(" | MapInput2=");
Serial.print(Input2);
Serial.print(" | Target2=");
Serial.print(Setpoint2);
Serial.print(" | OUT2=");
Serial.println(Output2);
//Serial.println(Input2);
delay(64*2); //wait 2 ms, the delay must be multiplied by 64 because we changed thme TCCR0B timer value of the arduino
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
void emergencyStop()
{
motor1.stopper(); // in case of emergency we stop the motor
motor2.stopper(); // in case of emergency we stop the motor
delay(10);
while(true) // and go into an infinite loop, so the user need to restart the arduino
{
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
void serialread()
{
if (Serial.available() > 0) //check for new character
{
c = Serial.read(); //store the character
if(c!=13 && nb_char<COMMAND_SIZE) //if not the end of a command, store it into aWord
{
aWord[nb_char] =c;
nb_char++;
}
else //if yes, start the command function
{
command(aWord,nb_char);
nb_char =0; // clear aWord
aWord[nb_char] ='\0';
Serial.flush(); //flush the Serial
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
char entete[4];
bool ok=true;
void command(char * cmd, int nb_char)
{
ok=true; //if something goes wrong, ok is pull to wrong
if(nb_char<3) //the command should have at least 3 characters
{
return;
}
for(int i =0; i<3;i++) //store the header of the command
{
entete[i]= cmd[i];
}
entete[3]='\0';
//////////////////////////////////////////////////////////////////
if(strcmp(entete,"A00")==0) //if the header is A00
{
int X= getvalue(cmd, nb_char, 'X', &ok); //read the value after X
int Y= getvalue(cmd, nb_char, 'Y', &ok); //read the value after Y
if(ok)
{
X=X*8-896; //we multiplie the value by 8 to improve the sensation, could be done in XSim
Y=Y*8-896; //we multiplie the value by 8 to improve the sensation, could be done in XSim
if(X<0)X=0; //we check is X is not to low
if(Y<0)Y=0; //we check is X is not to low
if(X>255)X=255; //or to high
if(Y>255)Y=255; //or to high
Setpoint1 =X; //we store the new setpoint
Setpoint2 =Y; //we store the new setpoint
}
}
else
{
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///
int getvalue(char *cmd, int nb_char, char c, bool *ok) //this function store the decimal value written after the character c
{
int i=0, j=0;
char value[COMMAND_SIZE];
while( i<nb_char)
{
if(cmd[i] == c)break; //find the place where th character is
i++;
}
i++;
while( i<nb_char&& cmd[i] != ' ') //search for any blank
{
value[j] =cmd[i];//store the value
i++;
j++;
}
if(j == 0) *ok=false;
value[j]='\0';
return atof(value); //convert into an integer
}
Argentina
Argentina| Tronic's DIY dual Mosfet H-bridge (DSMhb) | Forum: Motor actuators and drivers | Author: tronicgr | Replies: 366 |
| MOTION SIMULATOR WITH ARDUINO MEGA 2560 | Forum: DIY motion simulator projects | Author: davidlbraga | Replies: 4 |
| 2DOF with Actuators | Forum: DIY motion simulator projects | Author: bsft | Replies: 5 |
| Tronic's AMC motor motion-controller with pwm/servo output | Forum: X-Simulator compatible hardware | Author: tronicgr | Replies: 1420 |
| 2dof-wiper-motor-car-simulator-by-DERIY-(SATO) | Forum: DIY motion simulator projects | Author: DERIY | Replies: 131 |
Return to X-Simulator compatible hardware
Users browsing this forum: No registered users and 1 guest