//NXTway-Gyro Segway balancing robot with Hitechnic gyro
//Play station control add by Techbricks.nl using a Mindsensors PSP-Nx-v3 joystick controller 13.04.2009
//Based on JanB 20.12.2008 (NXC Bricx 3.3)
//last update 18.09.2009 - add line SetSensorHTGyro(Gyro_port) ;
//Mindsensors psp-nx PSP controller library
#include "PSP-Nx-lib.nxc"
//NXT sensor port for Mindsensors psp-nx PSP controller
const byte SensorPort = IN_2;
#define ADDR 0x02
/*--------------------------------------
Controller button layout:
----------------------------------------
l_j_x left joystick x-axle
l_j_y left joystick y-axle
-------------------------------------- */
#define Gyro_port S3
//variables used for reading out PSP controller
int count=20;
int move=0, turn=0,m=0,t=0, M=0, T=0,t1=7;
psp currState;
//variables used for balaning the bot
int ng,gn=0,nw=0,rp=0,rg,lrp=0,rwi=0,md;
int GyroBiasCount,Gyro_value,GyroBias=0, batt;
long timer1;
string message, messageA, messageB;
//Read the values of the left jotstick
task psp_controller()
{
// Read state psp-nx PSP controller
PSP_ReadButtonState(SensorPort, ADDR, currState);
// read out left joystick - Y-axe
move = (currState.l_j_y);
// read out left joystick - X-axe
turn = - currState.l_j_x;
// Display the joystick values on the NXT screen;
message = "y: x: ";
messageA = NumToStr(move);
messageB = NumToStr(turn);
message = StrReplace(message, 3, messageA);
message = StrReplace(message,12 , messageB);
TextOut(0, LCD_LINE7, message, false);
//return the values to the bot balancing task
m=move / 16; //devide joystick y-axe by factor
t=turn / 5; //devide joystick x-axe by factor
// During the psp_controller task, statements of the balancing task are executed slower.
// The processor of the NXT is serving 2 tasks at the same time after all.
// Therefore, end the end of the psp_comtroller task, the cuclus delay is set back from 4 ms to 7 ms.
t1=7; //set cyclus delay back to 7 ms
}
task balansing()
{
while(true)
{
ng=SensorRaw(S3)-gn; nw+=ng; //body angle in radians/sec
rp=(MotorTachoCount(OUT_A)+MotorTachoCount(OUT_B))/2; //wheel position
M = (M*75+m*25)/100; //To let the bot lowly get used to the for- and backward movements
rg=rp-lrp; lrp=rp; rwi+=rg-M; //wheel speed + joystick forward value
if (abs(rwi)>15){gn-=sign(rwi); rwi=0;} //Gyro drift compensation.
md=(nw+ng*10+rp*4+rg*200)>>4; //pd contr. NXT small wheels
// md=(nw+ng*12+rp*5+rg*360)>>5; //pd contr. RXC large wheels
md+=sign(md)*48; //friction compensation default = 48
SetOutput(OUT_A, //PID motor control motor A
Power,
md-t, //balance correction - joystick turn value.
OutputMode,
OUT_MODE_MOTORON,
RunState,
OUT_RUNSTATE_RUNNING,
UpdateFlags,
UF_UPDATE_MODE+UF_UPDATE_SPEED);
SetOutput(OUT_B, //PID motor control motor A
Power,
md+t, //balance correction + joystick turn value.
OutputMode,
OUT_MODE_MOTORON,
RunState,
OUT_RUNSTATE_RUNNING,
UpdateFlags,
UF_UPDATE_MODE+UF_UPDATE_SPEED);
if (count==20) //ones in 20 cycles (= 5 times per second)
{
count=0;
// During the task psp_controller the balancing task runs slower.
// Therefore the cuclus delay is set from 7 ms to 4 ms.
t1=4; // set cyclus delay to 4 ms
StartTask(psp_controller);// get joystick values m & t
}
count+=1;
Wait(t1); //Delay to slow down the loop to about 100 times/sec (default = 7ms)
}
}
task main(){
//set RAW_MODE Gyro sensor port
SetSensorHTGyro(Gyro_port) ;
// Calculate the Giro Bias for the Hitechnic Gyro sensor.
// by measuring the Gyro value many times in 3 secondes.
// Gyro Bias is avarage Gyro value: Sum of values devide by time measured.
timer1 = CurrentTick(); // set timer1
GyroBiasCount = 0;
while (CurrentTick() < (3000+timer1)) {
// filter the sensor output
Gyro_value = SensorRaw(S3);
Wait(150);
GyroBiasCount = GyroBiasCount + 1;
gn = gn + Gyro_value;
PlayTone(TONE_B7, 5);
}
GyroBias = gn / GyroBiasCount; //Sum of values devide by time measured.
gn=GyroBias;
// Measure the battery voltage.
batt=BatteryLevel();
// Display calculated GyroBias an battery voltage on the NXT screen;
message = "Gbias Bat ";
messageA = NumToStr(gn);
messageB = NumToStr(batt);
message = StrReplace(message, 5, messageA);
message = StrReplace(message, 12, messageB);
TextOut(0, LCD_LINE1, message, false);
//calculating GyroBias completed!
PlayTone(TONE_B7, 100);
//start self balansing task
Precedes(balansing);
}
//end