text: #pragma config(Sensor, dgtl11, ARM_SENSOR,     sensorQuadEncoder)
  #pragma config(Motor,  port1,           ARM_1,         tmotorVex393_HBridge, openLoop)
  #pragma config(Motor,  port2,           ARM_2,         tmotorVex393_MC29, openLoop, reversed)
  //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  
  /**
  * Power Curve Displayer
  * Made by Jess Zarchi / 21S Team Captain
  * Code originally made for OGLLA
  *
  * TO USE:
  * Firstly, config the motor and sensor setup to your needs
  * Make sure when you raise the arm, you get positive numbers.
  * Configure void runMotors( ) to your needs
  *
  * Start code with the arm down. Then bring the arm up. Set
  * iMaxHeight equal to the max height your arm can mechanically go.
  *
  * Open:
  * Robot > Debugger Windows > LCD
  * Robot > Debugger Windows > Debugger Stream
  *
  * Select the speed you'd like to run the motors, and click 
  * run. Once you click run, the debugger stream will output
  * two values: the current sensor value and the time. You can
  * save this as a .csv and open it in excell to see the power 
  * curve.
  *
  * If you output 127, you will get a close to linear curve. This
  * is becaues you are giving the lift too much power. The lower the power
  * is, the bigger the curve will show. Tune the values in POWER[4] to get 
  * biggest curve while still making your lift go all the way up.
  */
  
  const int iMaxHeight = 100;
  int POWER[4] = {20, 40, 80, 127};
  
  /**
  * Run Motors
  */
  void
  runMotors( int iSpeed ) {
  	motor[ ARM_1 ] = 
  	motor[ ARM_2 ] = iSpeed;
  }
  
  /**
  * Globalz
  */
  const int kMinNumberOfPages = 0;
  const int kMaxNumberOfPages = 3;
  int iPage = kMinNumberOfPages;
  bool bIsRunning;
  
  /**
  * Clear all LCD lines
  */
  void
  clearLCDLines( ) {
  	clearLCDLine( 0 );
  	clearLCDLine( 1 );
  }
  
  /**
  * Wait for LCD button to be released
  */
  void
  waitForRelease( ) {
  	while(nLCDButtons!=0){ }
  }
  
  task 
  main( ) {
  	SensorValue[ ARM_SENSOR ] = 0;
  	clearDebugStream( );
  	while( true ) {
  		if( !bIsRunning ) {
  			if( nLCDButtons == 1 ) {
  				iPage = iPage == kMinNumberOfPages ? kMaxNumberOfPages : iPage - 1;
  				waitForRelease( );
  			}
  			if( nLCDButtons == 4 ) {
  				iPage = iPage == kMaxNumberOfPages ? kMinNumberOfPages : iPage + 1;
  				waitForRelease( );
  			}
  			if( nLCDButtons == 2 ) {
  				waitForRelease( );
  				bIsRunning = true;
  			}
  			displayLCDCenteredString( 0, "POWER " );
  			displayLCDNumber( 0, 11, POWER[iPage] );
  			displayLCDCenteredString( 1, "<     RUN     >");
  			delay( 50 );
  			clearLCDLines( );
  		}
  		else {
  			displayLCDCenteredString( 0, "RUNNING" );
  			displayLCDString( 1, 3, "POWER " );
  			displayLCDNumber( 1, 10, POWER[iPage] );
  			int iTime = 0;
  			while( SensorValue[ ARM_SENSOR ] < iMaxHeight ) {
  				runMotors(POWER[iPage]);
  				writeDebugStreamLine("%i, %i", SensorValue[ ARM_SENSOR ], iTime);
  				delay( 10 );
  				iTime += 10;
  				if( !(SensorValue[ ARM_SENSOR ] < iMaxHeight) )
  					break;
  			}
  			bIsRunning = false;
  			runMotors( 0 );
  		}
  	}
  }
  