drive_panel.c

/*********************************************
 * Control panel for driving device. 
 * Objectives of this controller are: 
 * * Swap between two wheel drive and four wheel drive
 * * Smooth driving
 * * Speed control from separate input
 * * Display the speed on the 4 digit output
 * * Lights and light sequence when you press joystick button
 * * Button that moves a servo - will be used for firing later
 */

// CONSTANTS
#include <TM1637.h> // 4 digit display - see Grove 4 digit for examples
#include <Cytron_SmartDriveDuo.h>

#define CLK 8 // 4 digit display
#define DIO 9 // 4 digit display
#define IN1 3 // Arduino pin 3 is connected to MDDS30 pin IN1. 
#define AN1 5 // Arduino pin 5 is connected to MDDS30 pin AN1. 
#define AN2 6 // Arduino pin 6 is connected to MDDS30 pin AN2.
#define IN2 11 // Arduino pin 11 is connected to MDDS30 pin IN2. 
#define POT A0 // speed potentiometer
#define XR A1 //joystick x axis
#define YR A2 //joystick y axis
#define JS 10 //joystick switch
#define TRG 2 // Trig pin for distance sensor
#define ECO 4 // Echo pin for distance sensor


// Pins definitions for TM1637 (4 digit display)
TM1637 tm1637(CLK, DIO);
// motor driver
Cytron_SmartDriveDuo smartDriveDuo30(PWM_INDEPENDENT, IN1, IN2, AN1, AN2);


static int top_speed = 100; // 100 is the highest input to the smart drive
static int dt = 200; // delay timer
int current_speed = 100; // stores current speed global variable
int fwdbwd = 1; // used to toggle forward (positive) and backwards negative.
signed int speedLeft, speedRight; // right motor and left motor speed display
int distance = 0;

void setup() {
  //delay(2000);
  // put your setup code here, to run once:
  tm1637.init();
  tm1637.set(BRIGHT_TYPICAL);//BRIGHT_TYPICAL = 2,BRIGHT_DARKEST = 0,BRIGHTEST = 7;
  smartDriveDuo30.control(0,0);
  Serial.begin(9600);
  pinMode(XR, INPUT); 
  pinMode(YR, INPUT);
  pinMode(JS, INPUT);
//  pinMode(IN1, INPUT);
//  pinMode(IN2, INPUT);
//  pinMode(AN1, INPUT);
//  pinMode(AN2, INPUT);
  digitalWrite(JS, HIGH);
    // Setup for distance sensor
  pinMode(TRG, OUTPUT);
  pinMode(ECO, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  // 1. Set speed and write to display
  //setCurrentSpeed();
  current_speed = 50;
  //writeNumberToDisplay(current_speed);
  // 2. Read Joystick Inputs
  int xAxis = analogRead(XR); // Read Joysticks X-axis
  int yAxis = analogRead(YR); // Read Joysticks Y-axis
  printJoystickPosition(xAxis, yAxis);
  checkJoystickButton();
  // 3. Drive Loop
  // 3.1 Set in forward / backward motion
  if(xAxis < 265 && yAxis < 265){
    // NORTH WEST move to 50 / 100
    speedLeft = nextValue(0.5*top_speed, speedLeft);
    speedRight = nextValue(top_speed, speedRight);    
  } else if(xAxis < 777 && yAxis < 265){
    // NORTH move to 100 / 100
    speedLeft = nextValue(top_speed, speedLeft);
    speedRight = nextValue(top_speed, speedRight);    
  } else if(xAxis >= 777 && yAxis < 265){
    // NORTH EAST move to 100 / 50
    speedLeft = nextValue(top_speed, speedLeft);
    speedRight = nextValue(0.5*top_speed, speedRight);    
  }  
  
  else if(xAxis < 265 && yAxis >= 777){
    // SOUTH WEST move to -50 / -100
    speedLeft = nextValue(-0.5*top_speed, speedLeft);
    speedRight = nextValue(-top_speed, speedRight);    
  } else if(xAxis < 777 && yAxis >= 777){
    // SOUTH move to -100 / -100
    speedLeft = nextValue(-top_speed, speedLeft);
    speedRight = nextValue(-top_speed, speedRight);    
  } else if(xAxis >= 777 && yAxis >= 777){
    // SOUTH EAST move to 100 / 50
    speedLeft = nextValue(-top_speed, speedLeft);
    speedRight = nextValue(-0.5*top_speed, speedRight);    
  }  
  
  else if(xAxis < 265){
    // WEST move to -100 / 100
    speedLeft = nextValue(0.5*top_speed, speedLeft);
    speedRight = nextValue(-0.5*top_speed, speedRight);    
    
  } else if(xAxis >= 777){
    // EAST move to 100 / -100
    speedLeft = nextValue(-0.5*top_speed, speedLeft);
    speedRight = nextValue(0.5*top_speed, speedRight);    
    
  } else {
    // CENTER move to 0 / 0
    speedLeft = 0;
    speedRight = 0;
  }

  distance = checkDistance();
  // check distance sensor
  if(distance <= 0){
    //something wrong - ignore
  } else if(distance < 5){
    // do not permit forward motion
    Serial.println("Too close: Going to crash!");
    if(speedLeft < 0 || speedRight < 0){
      speedLeft = 0;
      speedRight = 0;
    }
  } else if(distance < 10){
    // reduces speeed in half
    Serial.println("Collision detection.");
    speedLeft = speedLeft/2;
    speedRight = speedRight/2;
    
  }
  
  printSpeed(speedRight, speedLeft);
  writeSpeedToDisplay(speedRight, speedLeft);
  smartDriveDuo30.control(speedRight, speedLeft);
    
  // N. Loop delay
  delay(dt);

}

// gradual change
int nextValue(int raw_reading, int last_reading){
  return ((last_reading*7)/8)+ (raw_reading/8);
}





// joystick button action
void checkJoystickButton(){
  int sValue = digitalRead(JS); // Read Joystick Switch
//  Serial.print("Button:  ");
//  Serial.print(sValue,DEC); // 1 is not pressed, 0 is pressed

  if(sValue == 1){
    return;
  } else {
    return;
  }
}

/*************************************************
* HELPER FUNCTIONS
*************************************************/

 /** PRINTING JOYSTICK POSITION
  *  Calibration 
  *  Forward = x,y 0,511 
  *  Backward = 1023, 511
  *  Left = 537,0
  *  Right = 537,1023
  *  Very sensitive
  */
void printJoystickPosition(int xAxis, int yAxis){
   Serial.print("Position (x,y) - Switch: ");
   Serial.print(xAxis,DEC);
   Serial.print(" ");
   Serial.print(yAxis,DEC);
   Serial.print("\n");

}

// Return distance (int) in cm to object from distance sensor
int checkDistance(){
  digitalWrite(TRG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRG, LOW);

  int duration = pulseIn(ECO, HIGH);
  return (int)duration*0.034/2;
}

void printSpeed(int right, int left){
   Serial.print("Speed (l,r): ");
   Serial.print(left,DEC);
   Serial.print("   ");
   Serial.print(right,DEC);
   Serial.print("\n");

}


// writes speed to display
void writeNumberToDisplay(int arg1){
    tm1637.displayNum(arg1*1.1);    // -123
}

// writes speed to display
void writeSpeedToDisplay(int right, int left){
    int temp = (right + left)/2;
    tm1637.displayNum(temp);    // -123
}



// reads input from pot and stores in variable current_speed
// only used with POT 
void setCurrentSpeed(){
  int temp_speed = map(analogRead(POT),0,1023,10,(top_speed*1.11));
  current_speed = ((current_speed*7)/8) + (temp_speed/8);
  Serial.println(temp_speed);
}