mindsensors-lineleader-test5.c

#pragma config(Sensor, S1,     LLEADER,             sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S4,     NUMPAD,              sensorI2CCustomFastSkipStates)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * $Id: mindsensors-lineleader-test5.c 133 2013-03-10 15:15:38Z xander $
 */

/** \file mindsensors-lineleader-test5.c
 * \brief Mindsensors LineLeader Sensor demo program
 *
 * MSLL-test5.c is a demo program for the Mindsensors LineLeader Sensor.
 *
 * Changelog:
 * - 0.1: Initial release
 * - 0.2: Reworked to use new driver API
 * - 0.3: More comments<br>
 *        Use clip() instead of manual clipping
 * - 0.4: Removed common.h from includes\n
 *        Replaced arrays with new type that does not use structs
 *
 * Credits:
 * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where its due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. 

 * Xander Soldaat (xander_at_botbench.com)
 * 14 February 2011
 * version 0.4
 */

#define MSLL_I2C_ADDR 0x02

#include "drivers/mindsensors-lineleader.h"
#include "drivers/mindsensors-numericpad.h"

// The PID constants and base speed
#define DEFAULT_SP   50

// The file for the log data
#define LOGFILE   "linelead.dat"

#define MENUITEMS 5

// function prototypes
void doMainMenu();
void doMenuItem(int activeOption);
bool checkTimer(TTimers timer);
void doLineLead();
void writeParams();

string menuHeader;
string menuFooter;

const TTimers rightButtonTimer = T1;
const TTimers leftButtonTimer  = T2;

int activeOption = 0;

int keep_running = 0;

byte sensor = 0;

// array to hold user entered values
int params[4];

string optionMainMenu[] = {
                              "Kp",
                              "Ki",
                              "Kd",
                              "Sp",
                              "Run" };

string optionMainMenuFooter[] = {
                             //123456789012345678
                              "*=Exit     #=Edit",
                              "*=Exit     #=Edit",
                              "*=Exit     #=Edit",
                              "*=Exit     #=Edit",
                              "*=Exit     #=Run " };

tByteArray signalstr;


int getNumber(tSensors link, int &_number, int numdigits=3, int timeout=0) {
  long starttime = nPgmTime;
  int tmpnum, prevnum, counter = 0;

  //_number = 0;

  while (true) {
    if ((timeout != 0) && nPgmTime > (starttime + timeout))
      return -1;

    // Read the keypad
    tmpnum = MSNPscanKeys(NUMPAD);

    // Reset the timer if a key has been pressed
    if (tmpnum != -255) {
			starttime = nPgmTime;
    }

    // Make sure we're not going beyond the requested length
    if ((counter >= numdigits) && (tmpnum >= 0))
      continue;

    if ((tmpnum >= 0) && (prevnum != tmpnum)) {
			PlaySound(soundBlip);
			while(bSoundActive) EndTimeSlice();
      if ((counter == 0) && (tmpnum >= 0)) {
        _number = tmpnum;
        counter++;
      } else {
        _number = _number*10 + tmpnum;
        counter++;
      }
    } else if (tmpnum == -1) {
      return (counter > 0) ? 0 : -1;
    } else if (tmpnum == -2) {
      return -2;
    }

    prevnum = tmpnum;
  }
  return 0;
}

//task batteryMonitor() {
//  while (true) {
//    if (nAvgBatteryLevel < 6200) {
//      PlaySound(soundBeepBeep);
//      while(bSoundActive) EndTimeSlice();
//      PlaySound(soundBeepBeep);
//      while(bSoundActive) EndTimeSlice();
//      StopAllTasks();
//    }
//    wait1Msec(500);
//  }
//}

// This function draws the current values of the sensor and other data on the screen
// in a visually pleasing way
task drawSensors() {
  while (keep_running == 1) {
    // This clears the entire area occupied by the small rectangles
    nxtEraseRect(6,62, 91, 43);
    for (int i = 0; i < 8; i++) {
      // Draw the rectangles for the signal strength first
      nxtDrawRect(6+(i*11),62, 14+(i*11), 50);
      nxtFillRect(6+(i*11),51+signalstr[i]/10, 14+(i*11), 50);
      // Draw a black rectangle if the sensor has detected the line,
      // a hollow one when nothing has been detected.
      if ((sensor >> i) & 1) {
        nxtFillRect(6+(i*11),48, 14+(i*11), 43);
      } else {
        nxtDrawRect(6+(i*11),48, 14+(i*11), 43);
      }
    }
    wait1Msec(100);
  }
}


// Keep the robot on the line!
task followTheYellowBrickRoad () {
  int powerA = 0;
  int powerC = 0;
  byte steering = 0;

  eraseDisplay();
  nxtDisplayCenteredTextLine(3, "Running...");
  nxtDisplayCenteredTextLine(5, "Press exit or *");
  nxtDisplayCenteredTextLine(6, "to stop");

  time1[T4] = 0;
  while (keep_running == 1) {
    if (MSNPscanKeys(NUMPAD) == -2)
      keep_running = 0;

    steering = LLreadSteering(LLEADER);
    sensor = LLreadResult(LLEADER);
    //average = LLreadAverage(LLEADER);
    LLreadSensorRaw(LLEADER, signalstr);

    steering /= 2;

    powerA = (params[3] - steering);
    powerC = (params[3] + steering);

    // If your robot is going in the wrong direction, comment out the
    // lines above and uncomment the lines below.
    //powerA = (params[3] - steering);
    //powerC = (params[3] + steering);

    // this clips the values
    powerA = clip(powerA, -100, 100);
    powerC = clip(powerC, -100, 100);

    //if(powerA>100)  powerA=100;
    //if(powerA<-100) powerA=-100;
    //if(powerC<-100) powerC=-100;
    //if(powerC>100)  powerC=100;

    motor[motorA] = -(byte)powerA;
    motor[motorC] = -(byte)powerC;
		wait1Msec(1);
		if (sensor != 0xFF) {
		  time1[T4] = 0;
	  } else if (time1[T4] > 500) {
	    keep_running = 0;
	  }
  }

  motor[motorA] = 0;
  motor[motorC] = 0;
}

task redrawMenu() {
  while(true) {
    eraseDisplay();
    nxtDisplayTextLine(0, menuHeader);
    for (int i = 0; i < MENUITEMS; i++) {
      nxtDisplayClearTextLine(i + 1);
      if (i == activeOption) {
        if (i < 4)
          nxtDisplayTextLine(i + 1, "> %s [%3d] <", optionMainMenu[i], params[i]);
        else
          nxtDisplayTextLine(i + 1, "> %s      <", optionMainMenu[i]);
        nxtDisplayTextLine(7, menuFooter);
      } else {
        if (i < 4)
          nxtDisplayTextLine(i + 1, "  %s [%3d]", optionMainMenu[i], params[i]);
        else
          nxtDisplayTextLine(i + 1, "  %s", optionMainMenu[i]);
      }
    }
    wait1Msec(100);
  }
}

// Main task
task main () {

//  memcpy(params, default_params, sizeof(default_params));

  params[0] = LLreadKp(LLEADER);
  params[1] = LLreadKi(LLEADER);
  params[2] = LLreadKd(LLEADER);
  params[3] = DEFAULT_SP;

  // Set the default parameters and write them to the sensor


  //writeParams();

  nNxtButtonTask  = -2;
  nNxtExitClicks = 3;

  StartTask(redrawMenu);
  doMainMenu();
  while(true)
    wait1Msec(100);
}

// Draw the main menu
void doMainMenu () {
  while (true) {
    menuHeader = "  2/8 => up/down";

    //StringFormat(menuFooter, "%s", optionMainMenuFooter[activeOption]);
    menuFooter = optionMainMenuFooter[activeOption];

    switch(MSNPscanKeys(NUMPAD)) {
      case 8:
            if (!checkTimer(rightButtonTimer)) {
              break;
            }
            if (activeOption == (MENUITEMS - 1))
              activeOption = 0;
            else
              activeOption++;
            //StringFormat(menuFooter, "%s", optionMainMenuFooter[activeOption]);
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            break;
      case 2:
            if (!checkTimer(leftButtonTimer)) {
              break;
            }
            if (activeOption == 0)
              activeOption = (MENUITEMS - 1);
            else
              activeOption--;
            //StringFormat(menuFooter, "%s", optionMainMenuFooter[activeOption]);
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            break;
      case -1:
            if (!checkTimer(rightButtonTimer)) {
              break;
            }
            wait1Msec(600);
            doMenuItem(activeOption);
            break;
      case -2:
            wait1Msec(500);
            StopAllTasks();

    }
  }
}

// When enter is pressed in the doMainMenu() we come here.
// Here we handle the actual actions for each menu item.
void doMenuItem(int activeOption) {
  long oldnumber = 0;
  int retval = 0;

  PlaySound(soundBlip);
  while(bSoundActive) EndTimeSlice();

  if (activeOption == 4) {
    doLineLead();
    return;
  }

  if (activeOption == 3)
	  menuHeader = "Enter nr (0-100)";
	else
	  menuHeader = "Enter nr (0-127)";

	            //123456789012345678
	menuFooter = "*=No Save  #=Save";

	oldnumber = params[activeOption];

	retval = getNumber(NUMPAD, params[activeOption], 3, 5000);

	switch (retval) {
	  case -2:  // aborted
              params[activeOption] = oldnumber;
              writeParams();
	            wait1Msec(1000);
	            break;
	   case -1: // Nothing entered
	            params[activeOption] = oldnumber;
	            writeParams();
	            wait1Msec(1000);
	            return;
	            break;
	   case 0:  // A number was entered
              if (activeOption == 3 && params[activeOption] > 100 )
                params[activeOption] = oldnumber;
              else if (params[activeOption] > 127)
                params[activeOption] = oldnumber;
              writeParams();
	            wait1Msec(1000);
	            return;
	            break;
	 }
}

// See if more than 300ms has elapsed
bool checkTimer(TTimers timer) {
  if (time1[timer] < 300) {
    return false;
  } else {
    time1[timer] = 0;
    return true;
  }
}

// Start and stop the line following task (followTheYellowBrickRoad)
void doLineLead() {
  sensor = 0;
  StopTask(redrawMenu);
  keep_running = 1;
  eraseDisplay();
  // Count down and
  for (int i = 0; i < 5; i++) {
    nxtDisplayCenteredBigTextLine(3, "%d", 5-i);
    PlaySound(soundBlip);
    wait1Msec(600);
  }
  PlaySound(soundFastUpwardTones);
  while(bSoundActive) EndTimeSlice();
  StartTask(drawSensors);
  StartTask(followTheYellowBrickRoad);
  while(nNxtButtonPressed != kExitButton && keep_running != 0) {
    wait1Msec(10);
  }
  // this will kill off the followTheYellowBrickRoad task
  keep_running = 0;
  wait1Msec(1000);
  StartTask(redrawMenu);
}

// Write the PID values to the LineLeader sensor
void writeParams() {
  LLsetKp(LLEADER, params[0], 32);
  LLsetKi(LLEADER, params[1], 32);
  LLsetKd(LLEADER, params[2], 32);
}

/*
 * $Id: mindsensors-lineleader-test5.c 133 2013-03-10 15:15:38Z xander $
 */