Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //Pin defenitions and other essential values
- #define pwm_r 5 //PWM control for motor outputs 1 and 2 is on digital pin 5 RIGHT
- #define pwm_l 11 //PWM control for motor outputs 3 and 4 is on digital pin 11 LEFT
- #define dir_r 12 //dir control for motor outputs 1 and 2 is on digital pin 12 RIGHT Low is forward
- #define dir_l 13 //dir control for motor outputs 3 and 4 is on digital pin 13 LEFT High is forward
- #define PINGA 9 //Trigger and echo for the Front Sensor
- #define PINGB 8 // Echo for the Front Sensor
- #define BASE_SPEED 128 // base speed of the robot
- #define SETPOINT 10 //setpoint
- // Variables
- long sonica = 0; //Sonic a intermediate distance
- long sonicb = 0;// sonic b intermediate distance
- volatile int counterL = 0; // Counter for the Left Wheel encoder
- volatile int counterR = 0; // Counter for the Right Wheel encoder
- double rspeed = 128;
- double lspeed = 128;
- double SetPoint = 10;
- double ratio = 1;
- double Kp = .25;
- double diff = 0;
- double spchange = 0;
- double Max = 0;
- double Min = 0;
- double max_dist = 25;
- double integral = 0;
- double Ki = .25;
- void setup()
- {
- attachInterrupt(0, AddL, CHANGE); // Intettupt for the Left Wheel encoder
- attachInterrupt(1, AddR, CHANGE); // Interrupt for the Right Wheel encoder
- pinMode(5, OUTPUT); //Set control PWM pins to be outputs Speed variance
- pinMode(11, OUTPUT);
- pinMode(dir_r, OUTPUT); // Set the direction pins to Output (high or low)
- pinMode(dir_l, OUTPUT);
- Serial.begin(9600); //Opens up a serial port for command rx and data tx
- Max = BASE_SPEED+((max_dist - SetPoint)*Kp)*BASE_SPEED;
- Min = -Max;
- }
- void AddL(){ // Addition for the left wheel
- counterL++;
- }
- void AddR(){ //Addition for the right wheel
- counterR++;
- }
- void loop()
- {
- sonica = Sonic(PINGA); // Collect a sample from the front sensor
- sonicb = Sonic(PINGB); // Collect a sample from the right sensor
- if (sonica >= 35){
- if(SETPOINT == sonicb){
- ratio = 1;
- }
- else if (SETPOINT != sonicb){
- diff = SETPOINT - sonicb;
- integral +=diff;
- ratio = (diff*Kp)+(integral*Ki);
- }
- spchange = BASE_SPEED*ratio;
- rspeed += spchange;
- lspeed -= spchange;
- rspeed = map(rspeed, Min, Max, 0, 255);
- lspeed = map(lspeed, Min, Max, 0, 255);
- if (ratio == 1){
- rspeed = BASE_SPEED;
- lspeed = BASE_SPEED;
- }
- }
- else{
- rspeed = BASE_SPEED;
- lspeed = 0;
- }
- if( sonicb <= 6){
- rspeed = BASE_SPEED;
- lspeed = 0;
- }
- if(rspeed >255)
- rspeed = 255;
- if(lspeed > 255)
- lspeed = 255;
- if(rspeed < 0)
- rspeed = 0;
- if(lspeed < 0)
- lspeed = 0;
- SetWheel(pwm_r, rspeed, dir_r, LOW);
- SetWheel(pwm_l, lspeed, dir_l, HIGH);
- delay(100);
- //Serial.print(counterR);
- //Serial.print(",");
- //Serial.println(counterL);
- // Serial.print(sonica);
- //Serial.print(",");
- //Serial.println(sonicb);
- // Serial.print(",");
- Serial.print(rspeed);
- Serial.print(",");
- Serial.println(lspeed);
- counterR = 0;
- counterL = 0;
- // rspeed = BASE_SPEED;
- // lspeed = BASE_SPEED;
- delay(100);
- }
- // Methods to control the motors
- void SetWheel( int pwmpin, int vel, int dirpin, boolean dir){
- analogWrite(pwmpin, vel);
- digitalWrite(dirpin, dir);
- }
- //Method to read/write data to and from the sonic sensors, The below is setup per the PING))) Datasheet
- long Sonic(int a){
- long duration, cm;
- pinMode(a, OUTPUT);
- digitalWrite(a, LOW);
- delayMicroseconds(2);
- digitalWrite(a, HIGH);
- delayMicroseconds(5);
- digitalWrite(a, LOW);
- pinMode(a, INPUT);
- duration = pulseIn(a, HIGH);
- delay(10);
- cm = (duration/29) /2;
- return cm;
- }
Add Comment
Please, Sign In to add comment