Playbook: Joystick Programming -- The Exercise

Purpose: Learning how to program based on input from devices.

The goal of this exercise is to be able to create the necessary functions to control a robot using the joystick.

Requirements

This exercise requires you to be in front of a computer with RobotC and a Mindstorms robot. You will write a program according to the instructions below. Try not to just jump to the end of the lesson and copy the whole program. You won't learn much by doing that. Follow the individual steps shown below. Follow them in order--so that you'll know what to do when you have to write your own programs some day.

Go to the Motors and Sensors setup and configure two motors: B and C. Configure two light sensors on ports 2 and 3 and a touch sensor on port 1.

When configuring motor B, just give it the name "B". When configuring motor C, just give it the name "C".

When configuring touch sensor on port 1, just give it the name "touchSensor".
When configuring color sensor on port 2, just give it the name "colorSensor1".
When configuring color sensor on port 3, just give it the name "colorSensor2".
If you want to configure the Ultrasonic sensor on port 4, call it: ultrasonicSensor

Start RobotC and access a New program

Start RobotC then click on the New File button from the button bar. The new program should appear as:

task main()
{


}

#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S3,     colorSensor2,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

Then add a few more helper functions shown below which will create an object type to create coordinate variables that will be used in circles. Also the code below will create two definitions for trigonometry functions.

//####################################################
// Graphics Library Structures
typedef struct circle_struct { int x; int y; } new_Circle;
#define sine(Degrees) sinDegrees(Degrees)
#define cosine(Degrees) cosDegrees(Degrees)

When you have finished configuring the motors and sensors, you should see the code above added to the top of your program.

Insert a while() loop AND a few variables as shown below. Two circles are being created (one for power and one for steering). You can adjust the maximum power variable to suit your needs. However, for this demo, just leave it at 50% as shown.

task main()
{
  setJoystickScale(10);
  new_Circle powerCircle;
  new_Circle steeringCircle;
  int maximumPower=50;

  while(true) {
    getJoystickSettings(joystick);
    eraseDisplay();


  }

}

Now run your program.

Any error messages? I hope not. Nothing happened just yet. We still have to program that part. If it didn't compile and run, try copying the code below.

#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S3,     colorSensor2,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//####################################################
// Graphics Library Structures
typedef struct circle_struct { int x; int y; } new_Circle;
#define sine(Degrees) sinDegrees(Degrees)
#define cosine(Degrees) cosDegrees(Degrees)



task main()
{
  setJoystickScale(10);
  new_Circle powerCircle;
  new_Circle steeringCircle;
  int maximumPower=50;
  while(true) {
    getJoystickSettings(joystick);
    eraseDisplay();

  }

}

The joystick has a "top hat" button which is a small button on the top of the joystick. This button has 9 possible positions. However, we will just use the 5 main positions. Add a switch block inside the while() loop as shown below. Pay special attention to the punctuation, spelling and capitalization of the parameter in the switch block. Notice how the period separates the property "joy1_TopHat" from the object "joystick". These are not variables that you are creating in the switch. These are objects and properties that you can utilize in your program. These objects and properties already exist. So, in order to use them, you need to make sure you spell them correctly.

task main()
{
  setJoystickScale(10);
  new_Circle powerCircle;
  new_Circle steeringCircle;
  int maximumPower=50;
  while(true) {
    getJoystickSettings(joystick);
    eraseDisplay();

    switch(joystick.joy1_TopHat) {
    case 0:
      break;
    case 2:
      break;
    case 4:
      break;
    case 6:
      break;
    default:
    }
  }

}

Following each "case n:" in the switch block is a break; which tells the robot to stop processing that case and exit the switch block. Though the break; is not a "required" statement, it is highly advisable in this example.

The next step is to edit the switch block that you just entered. We are going to add a few functions inside of each case. Remember, we are editing the code--not adding a duplicate switch block.

Now add the following fillRect() functions to each case as shown below. Make sure you enter the parameters correctly. Each rectangle needs 4 parameters (x1, y1, and x2, y2). You do not have to define the fillRect() function. It is a built-in function that you can use.

///////////
    switch(joystick.joy1_TopHat) {
    case 0:
      fillRect(0,125,175,95);
      break;
    case 2:
      fillRect(175,125,135,0);
      break;
    case 4:
      fillRect(0,0,175,35);
      break;
    case 6:
      fillRect(0,125,30,0);
      break;
    default:
    }

Now run your program. What happens when you move the Top Hat on the joystick?

There are other buttons on the joystick. Let's add them to another switch block. Add the following to your program after the switch block already there.

/////////////
    switch(joystick.joy1_Buttons) {
    case 1:
      setLEDColor(ledRed);
      break;
    case 2:
      setLEDColor(ledOrange);
      break;
    case 4:
      setLEDColor(ledGreen);
      break;
    default:
      setLEDColor(ledOff);
      break;
    }

Notice how this switch block uses a different parameter: joystick.joy1_Buttons which is another property of the joystick object.

Run your program now.

Did it work?

I hope it did.

If not, try using the whole program below.

#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S3,     colorSensor2,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//####################################################
// Graphics Library Structures
typedef struct circle_struct { int x; int y; } new_Circle;
#define sine(Degrees) sinDegrees(Degrees)
#define cosine(Degrees) cosDegrees(Degrees)

task main()
{
  setJoystickScale(10);
  new_Circle powerCircle;
  new_Circle steeringCircle;
  int maximumPower=50;
  while(true) {
    getJoystickSettings(joystick);
    eraseDisplay();
    switch(joystick.joy1_TopHat) {
    case 0:
      fillRect(0,125,175,95);
      break;
    case 2:
      fillRect(175,125,135,0);
      break;
    case 4:
      fillRect(0,0,175,35);
      break;
    case 6:
      fillRect(0,125,30,0);
      break;
    default:
    }
    switch(joystick.joy1_Buttons) {
    case 1:
      setLEDColor(ledRed);
      break;
    case 2:
      setLEDColor(ledOrange);
      break;
    case 4:
      setLEDColor(ledGreen);
      break;
    default:
      setLEDColor(ledOff);
      break;
    }
  }
}

Now that you have all of the buttons working. What about the joystick itself?

Notice that lonely default: in the first switch block. Let's add some statements to be executed if the "top hat" is not selected.

Add the following to the default: case in the first switch block.

///////////////
      int radius;
      radius=20+abs(joystick.joy1_y1);
      powerCircle.x = 75-radius/3;
      powerCircle.y = 60+radius/3;
      drawCircle(powerCircle.x,powerCircle.y,radius);
      steeringCircle.x = 75 + cosine(joystick.joy1_y2*180/250+90) * radius/2;
      steeringCircle.y = 60 + sine(joystick.joy1_y2*180/250+90) * radius/2;
      drawCircle(steeringCircle.x,steeringCircle.y,20);
      sleep(20);

Now the first switch block should look like:

//////////
    switch(joystick.joy1_TopHat) {
    case 0:
      fillRect(0,125,175,95);
      break;
    case 2:
      fillRect(175,125,135,0);
      break;
    case 4:
      fillRect(0,0,175,35);
      break;
    case 6:
      fillRect(0,125,30,0);
      break;
    default:
      int radius;
      radius=20+abs(joystick.joy1_y1);
      powerCircle.x = 75-radius/3;
      powerCircle.y = 60+radius/3;
      drawCircle(powerCircle.x,powerCircle.y,radius);
      steeringCircle.x = 75 + cosine(joystick.joy1_y2*180/250+90) * radius/2;
      steeringCircle.y = 60 + sine(joystick.joy1_y2*180/250+90) * radius/2;
      drawCircle(steeringCircle.x,steeringCircle.y,20);
      sleep(20);
    }

Let's look a little closer at the default: case we just updated.

You will notice that a "radius" variable is created as an integer. The radius is set to a minimum of 20 pixels. We are then adding to that minimum the absolute value of the joystick's primary Y-axis. This value is another property of the joystick object. This property is called: joy1_y1

The joystick.joy1_y1 property has a range of values from -127 to +127. When we calculate the absolute value of this property, the values we obtain are from 0 to 127. When the joystick is centered, the value returned is 0. If the joystick is pushed forward, the value increases to 127. If it is pulled back, the value decreases to -127. Calculating the absolute value abs() of this property tells us the amount of displacement from the center position. We will show that displacement visually through the radius of a large circle centered in the LCD panel.

The X and Y coordinates of the powerCircle are set to roughly the middle of the LCD screen. The coordinates shown here are not the center of the circle, but rather the upper left corner of an imaginary box that encompasses the circle. As the circle expands, so does the box and therefore the upper left corner needs re-positioned accordingly. Notice how the x property of the powerCircle is the center point minus the radius and the y property is the center point plus the radius. The reason why we subtract one and add the other is because the origin point of the LCD display is the lower left corner and the xy coordinate for the circle is the upper left corner. To make a larger circle, the upper left corner of the circle must have a higher value for the y coordinate than the center point.

The steeringCircle is a smaller circle that follows the path outlined by the larger powerCircle. So, the position of the steeringCircle is determined by the sine and cosine from the center point (75, 60). The position of the steering circle changes with the rotation of the joystick (secondary Y-axis) and the radius of the powerCircle (divided by 2).

Add motor commands to the first switch block as shown below:

/////////////////
    switch(joystick.joy1_TopHat) {
    case 0:
      fillRect(0,125,175,95);
      motor[B]=20;
      motor[C]=20;
      break;
    case 2:
      fillRect(175,125,135,0);
      motor[B]=20;
      motor[C]=-20;
      break;
    case 4:
      fillRect(0,0,175,35);
      motor[B]=-20;
      motor[C]=-20;
      break;
    case 6:
      fillRect(0,125,30,0);
      motor[B]=-20;
      motor[C]=20;
      break;
    default:
      int radius;
      radius=20+abs(joystick.joy1_y1);
      powerCircle.x = 75-radius/3;
      powerCircle.y = 60+radius/3;
      drawCircle(powerCircle.x,powerCircle.y,radius);
      steeringCircle.x = 75 + cosine(joystick.joy1_y2*180/250+90) * radius/2;
      steeringCircle.y = 60 + sine(joystick.joy1_y2*180/250+90) * radius/2;
      drawCircle(steeringCircle.x,steeringCircle.y,20);
      int motorPower;
      motorPower = maximumPower * joystick.joy1_y1 / 129;
      motor[B] = motorPower + motorPower * joystick.joy1_y2 / 129;
      motor[C] = motorPower - motorPower * joystick.joy1_y2 / 129;
      sleep(20);
    }

Run the program. Does it move?

There are a few more buttons that can be programmed. Add the following cases to the second switch block. Add these additional cases before the default: case.

////////////////////
    case 64:
      setLEDColor(ledOrangeFlash);
      sleep(1000);
      break;
    case 128:
      setLEDColor(ledRedFlash);
      sleep(1000);
      break;
    case 256:
      setLEDColor(ledGreenFlash);
      sleep(1000);
      break;
    case 512:
      setLEDColor(ledOrangePulse);
      sleep(1000);
      break;
    case 1024:
      setLEDColor(ledRedPulse);
      sleep(1000);
      break;
    case 2048:
      setLEDColor(ledGreenPulse);
      sleep(1000);
      break;

The Whole program should now appear as:

#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S3,     colorSensor2,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

//####################################################
// Graphics Library Structures
typedef struct circle_struct { int x; int y; } new_Circle;
#define sine(Degrees) sinDegrees(Degrees)
#define cosine(Degrees) cosDegrees(Degrees)

task main()
{
  setJoystickScale(10);
  new_Circle powerCircle;
  new_Circle steeringCircle;
  int maximumPower=50;
  while(true) {
    getJoystickSettings(joystick);
    eraseDisplay();
    switch(joystick.joy1_TopHat) {
    case 0:
      fillRect(0,125,175,95);
      motor[B]=20;
      motor[C]=20;
      break;
    case 2:
      fillRect(175,125,135,0);
      motor[B]=20;
      motor[C]=-20;
      break;
    case 4:
      fillRect(0,0,175,35);
      motor[B]=-20;
      motor[C]=-20;
      break;
    case 6:
      fillRect(0,125,30,0);
      motor[B]=-20;
      motor[C]=20;
      break;
    default:
      int radius;
      radius=20+abs(joystick.joy1_y1);
      powerCircle.x = 75-radius/3;
      powerCircle.y = 60+radius/3;
      drawCircle(powerCircle.x,powerCircle.y,radius);
      steeringCircle.x = 75 + cosine(joystick.joy1_y2*180/250+90) * radius/2;
      steeringCircle.y = 60 + sine(joystick.joy1_y2*180/250+90) * radius/2;
      drawCircle(steeringCircle.x,steeringCircle.y,20);
      int motorPower;
      motorPower = maximumPower * joystick.joy1_y1 / 129;
      motor[B] = motorPower + motorPower * joystick.joy1_y2 / 129;
      motor[C] = motorPower - motorPower * joystick.joy1_y2 / 129;
      sleep(20);
    }

/////////////////
    switch(joystick.joy1_Buttons) {
    case 1:
      setLEDColor(ledRed);
      break;
    case 2:
      setLEDColor(ledOrange);
      break;
    case 4:
      setLEDColor(ledGreen);
      break;
    case 64:
      setLEDColor(ledOrangeFlash);
      sleep(1000);
      break;
    case 128:
      setLEDColor(ledRedFlash);
      sleep(1000);
      break;
    case 256:
      setLEDColor(ledGreenFlash);
      sleep(1000);
      break;
    case 512:
      setLEDColor(ledOrangePulse);
      sleep(1000);
      break;
    case 1024:
      setLEDColor(ledRedPulse);
      sleep(1000);
      break;
    case 2048:
      setLEDColor(ledGreenPulse);
      sleep(1000);
      break;
    default:
      setLEDColor(ledOff);
      break;
    }
  }
}