NXC w/ mindsensors PSP-Nx and Servos

[emanuellinux]

Hey guys,

This is my first time writing with NXC and/or object oriented programming in general. I am making a program that takes values from the Mindsensors PSP-Nx controller and moves servos using the Mindsensors NXTServo accordingly. Whenever I try to compile i find weird errors talking about parenthesis and semicolons being left out. I am not sure what it is directly talking about. Any ideas?

Code: Select all

#include "PSP-Nx-lib.nxc"
#include "NXTServo-lib.nxc"

const byte SensorPort  =  IN_1;
#define ADDR     0x02

#define ServoSensorPort S2
const byte ServoAddr = 0xb0;

int Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8;

int Pos_Rotate, Pos_Base, Pos_Mid, Pos_End, Pos_Gripper_Rotate, Pos_Gripper;

int Servo1Speed, Servo2Speed, Servo3Speed, Servo4Speed;
int Servo5Speed, Servo6Speed, Servo7Speed, Servo8Speed;

psp currState;
string msg, x;

task base_rotation ()        //This controls the rotating base
{
     if (PSP_format_bin(currState.r_j_y) != 0) {
        Servo1Speed = abs("PSP_format_bin(currState.r_j_y)");
        NXTServo_SetSpeed("ServoSensorPort", "ServoAddr", "Servo1", "Servo1Speed");
        while (PSP_format_bin(currState.r_j_y) > 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo1", "Pos_Rotate");
              Pos_Rotate += 2;
              Wait(100);
        }
        while (PSP_format_bin(currState.r_j_y < 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo1", "Pos_Rotate");
              Pos_Rotate -= 2;
              Wait (100);
        }
     }
}

task base_link ()            //This controls the first joint attached to base
{
     if (PSP_format_bin(currState.r_j_x) != 0) {
        Servo2Speed = abs("PSP_format_bin(currState.r_j_x)");
        Servo3Speed = abs("PSP_format_bin(currState.r_j_x)');
        NXTServoSpeed ("ServoSensorPort", "Servo2", "Servo2Speed");
        NXTServoSpeed ("ServoSensorPort", "Servo3", "Servo3Speed");
        while (PSP_format_bin(currState.r_j_x) > 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo2", "Pos_Base");
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo3", "Pos_Base");
              Pos_Base += 2;
              Wait (100);
        }
        while (PSP_format_bin(currState.r_j_x) < 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo2", "Pos_Base");
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo3", "Pos_Base");
              Pos_Base -= 2;
              Wait (100);
        }
     }
}

task middle_link ()          //This controls the middle joint
{
     if (PSP_format_bin(currState.l_j_x) != 0) {
        Servo4Speed = abs("PSP_format_bin(currState.l_j_x)");
        Servo5Speed = abs("PSP_format_bin(currState.l_j_x)");
        NXTServoSpeed ("ServoSensorPort", "Servo4", "Servo4Speed");
        NXTServoSpeed ("ServoSensorPort", "Servo5", "Servo5Speed");
        while (PSP_format_bin(currState.l_j_x) > 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo4", "Pos_Mid");
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo5", "Pos_Mid");
              Pos_Mid += 2;
              Wait (100);
        }
        while (PSP_format_bin(currState.l_j_x) < 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo4", "Pos_Mid");
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo5", "Pos_Mid");
              Pos_Mid -= 2;
              Wait (100);
        }
     }
}

task end_link ()             //This controls the end joint
{
     if (PSP_format_bin(currState.l_j_y) != 0) {
        Servo6Speed = abs("PSP_format_bin(currState.l_j_y)");
        NXTServoSpeed ("ServoSensorPort", "Servo6", "Servo6Speed");
        while (PSP_format_bin(currState.l_j_y) > 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort","ServoAddr", "Servo6", "Pos_End");
              Pos_End += 2;
              Wait (100);
        }
        while (PSP_format_bin(currState.l_j_y) < 0) {
              NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo6", "Pos_End");
              Pos_End -= 2;
              Wait (100);
        }
     }
}

task gripper_rotate ()       //This controls the gripper attachement rotation
{
     while (PSP_format_bin(currState.R1) == 1) {
           NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo7", "Pos_Gripper_Rotate");
           Pos_Gripper_Rotate ++
           Wait (100);
     }
     while (PSP_format_bin(currState.L2) == 1) {
           NXTServo_Quick_Servo_Setup("ServoSensorport", "ServoAddr", "Servo7", "Pos_Gripper_Rotate");
           Pos_Gripper_Rotate --
           Wait (100);
     }
}

task gripper_control ()      //This controls the gripper attachment servo
{
     while (PSP_format_bin(currState.L1) = 1) {
           NXTServo_Quick_Servo_Setup("ServoSensorPort", "ServoAddr", "Servo8", "Pos_Gripper");
           Pos_Gripper ++
           Wait (100);
     }
     while (PSP_format_bin(currState.L2) = 1) {
           NXTServo_Quick_Servo_Setup("ServoSensorPOrt", "ServoAddr", "Servo8", "Pos_Gripper");
           Pos_Gripper --
           Wait (100);
     }
}

task main ()
{
     Precedes(base_rotation, base_link, middle_link, end_link, gripper_control);
     Servo1 = 1, Servo2 = 2, Servo3 = 3, Servo4 = 4, Servo5 = 5, Servo6 = 6, Servo7 = 7, Servo8 = 8;

  while (true ) {            //Monitor controller power and display it's status

    PSP_ReadButtonState(SensorPort, ADDR, currState);

    msg = "b1: Controller Active! ";
    x = PSP_format_bin(currState.b1);
    msg = StrReplace(msg, 3, x);
    TextOut(0, LCD_LINE1, msg, false);
  }
}

Thanks

[afanofosc]

A large number of your function calls are passing "string" values where they should just be passing variable names (no quotes). You may be confused due to the way that the template file shows all of the function parameter values in quotation marks. This is an artifact of the original RCX Command Center which used what it called F10 Fields to "templatize" function calls. You would insert the template into your source code and then F10 through all of the "fields" and replace them with the desired value. Since NXC supports a string type it is not as obvious that these are not intended to be quoted strings. The BricxCC help actually says something about this (see the template topic when no editor windows are open).

In general, unless you are dealing with a function that actually takes a string type (which the F1 context-sensitive help can tell you as well as the Ctrl+Shift+Space parameter completion feature can tell you).

You can enable template drag-and-drop by turning on "double-click to insert" via the right-click context menu on the template window. Then drag and drop the template wherever you want it in the editor. The first field will be highlighted, e.g., abs("expression") with "expression" selected, so just start typing and it will replace the selected field with the expression that you type in.

John Hansen

[emanuellinux]

Thank you very much for clearing that up.

I have a few more (noob) questions though. To get the values for the joysticks (which should be integers) do I need to define l_j_x, l_j_y, r_j_x, and r_j_y as integers, or should they already be defined as so from the PSP-Nx library?
This is what I have so far, though the errors I am getting are consisting of stating that the above joystick and button values are undefined:

Code: Select all

#include "PSP-Nx-lib.nxc"
#include "NXTServo-lib.nxc"

const byte SensorPort  =  IN_1;
#define ADDR     0x02

#define ServoSensorPort S2
const byte ServoAddr = 0xb0;

int Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8;

int Pos_Rotate, Pos_Base, Pos_Mid, Pos_End, Pos_Gripper_Rotate, Pos_Gripper;

int Servo1Speed, Servo2Speed, Servo3Speed, Servo4Speed;
int Servo5Speed, Servo6Speed, Servo7Speed, Servo8Speed;

psp currState;
string msg, x;

task base_rotation ()        //This controls the rotating base
{
     if  (r_j_y != 0); {
        Servo1Speed = abs (r_j_y);
        NXTServo_SetSpeed(ServoSensorPort, ServoAddr, Servo1, Servo1Speed);
        while  (r_j_y > 0); {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo1, Pos_Rotate);
              Pos_Rotate += 2;
              Wait(100);
        }
        while  (r_j_y < 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo1, Pos_Rotate);
              Pos_Rotate -= 2;
              Wait (100);
        }
     }
}

task base_link ()            //This controls the first joint attached to base
{
     if  (r_j_x != 0); {
        Servo2Speed = abs(r_j_x);
        Servo3Speed = abs(r_j_x);
        NXTServoSpeed (ServoSensorPort, Servo2, Servo2Speed);
        NXTServoSpeed (ServoSensorPort, Servo3, Servo3Speed);
        while  (r_j_x > 0); {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo2, Pos_Base);
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo3, Pos_Base);
              Pos_Base += 2;
              Wait (100);
        }
        while  (r_j_x < 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo2, Pos_Base);
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo3, Pos_Base);
              Pos_Base -= 2;
              Wait (100);
        }
     }
}

task middle_link ()          //This controls the middle joint
{
     if  (l_j_x != 0) {
        Servo4Speed = abs(l_j_x);
        Servo5Speed = abs(l_j_x);
        NXTServoSpeed (ServoSensorPort, Servo4, Servo4Speed);
        NXTServoSpeed (ServoSensorPort, Servo5, Servo5Speed);
        while  (l_j_x > 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo4, Pos_Mid);
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo5, Pos_Mid);
              Pos_Mid += 2;
              Wait (100);
        }
        while  (l_j_x < 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo4, Pos_Mid);
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo5, Pos_Mid);
              Pos_Mid -= 2;
              Wait (100);
        }
     }
}

task end_link ()             //This controls the end joint
{
     if  (l_j_y != 0) {
        Servo6Speed = abs(l_j_y);
        NXTServoSpeed (ServoSensorPort, Servo6, Servo6Speed);
        while  (l_j_y > 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort,ServoAddr, Servo6, Pos_End);
              Pos_End += 2;
              Wait (100);
        }
        while  (l_j_y < 0) {
              NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo6, Pos_End);
              Pos_End -= 2;
              Wait (100);
        }
     }
}

task gripper_rotate ()       //This controls the gripper attachement rotation
{
     while  (R1 == 1); {
           NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo7, Pos_Gripper_Rotate);
           Pos_Gripper_Rotate++;
           Wait (100);
     }
     while  (L1 == 1); {
           NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo7, Pos_Gripper_Rotate);
           Pos_Gripper_Rotate--;
           Wait (100);
     }
}

task gripper_control ()      //This controls the gripper attachment servo
{
     while  (R2 == 1); {
           NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo8, Pos_Gripper);
           Pos_Gripper++;
           Wait (100);
     }
     while  (L2 == 1); {
           NXTServo_Quick_Servo_Setup(ServoSensorPort, ServoAddr, Servo8, Pos_Gripper);
           Pos_Gripper--;
           Wait (100);
     }
}

task main ()
{
     Precedes(base_rotation, base_link, middle_link, end_link, gripper_control);
     Servo1 = 1, Servo2 = 2, Servo3 = 3, Servo4 = 4, Servo5 = 5, Servo6 = 6, Servo7 = 7, Servo8 = 8;

  while (true ) {            //Monitor controller power and display it's status

    PSP_ReadButtonState(SensorPort, ADDR, currState);

    msg = "b1: Controller Active! ";
    x = "PSP_Format_Bin(currState.b1)";
    msg = StrReplace(msg, 3, x);
    TextOut(0, LCD_LINE1, msg, false);
  }
}

\

Thank You

[afanofosc]

Just to be clear, I don't use the mindsensors.com library routines at all so I'm not familiar with exactly how they are intended to work.

But, it does look like you need to use your instance of the psp structure (currState) followed by a period (.) and then followed by the structure member name (l_j_x) like you had it before except that it was quoted before. So, for example, where you have l_j_x you would want to have currState.l_j_x instead. The mindsensors.com website has sample programs that use their libraries and sometimes even HTML documents that describe how to use the library. You may want to read them.

You could use the NXC API functions for the PSP device if you want to, rather than using the mindsensors.com library. I am more familiar with them so I'd be better able to explain how to use them than what you are currently using. Unfortunately, I have not yet implemented a set of NXC API functions for the NXTServo device. It is on my long to-do list but I hope to get to it, along with their numeric pad and touch panel sometime in February or March.

One major question that I have is this: How did your program get so big when you don't have any idea how to write NXC code or use the mindsensors.com libraries? My approach to programming is to start small:

Code: Select all

task main()
{
}

If I can get that to compile and run without crashing the NXT then I add a little more code, try compiling it and running it, and continue building up my program bit by bit until it gets to be large but at no time is it filled with lots of errors that could never have even thought of compiling successfully.

One other thing you should know is that if task main never ends (i.e., it contains a while (true) loop that never exits) then none of the tasks that it precedes will ever start executing.

John Hansen

[emanuellinux]

Honestly, the team I am on is on a deadline so we took what programming knowledge we do know and combined it to try to come up with what we thought would work. It obviously didn't/hasn't. We are just trying to get a working program. I am more than willing to use the built in commands for the Mindsensors products. I can find commands for the NXTServo but can't for the PSP-Nx, unless I am looking in the wrong place?

I will take your advice and start small, though I need a working program relatively quickly.

Thank You

[emanuellinux]

Well, I went back and looked at the online documentation I could find and tried what you suggested by starting simple. I was able to get the servos to move with just basic NXTServo_Quick_Servo_Setup() commands, but I was met with problems when trying to incorporate the PSP-Nx into the mix. Do I have the I2C addresses messed up, or maybe I am using the wrong commands? I am lost at the moment, anybody willing to point me in the right direction to get this going?

This is what I currently have:

Code: Select all

#include "PSP-Nx-lib.nxc"
#include "NXTServo-lib.nxc"

const byte SensorPort  =  IN_2;
#define ADDR     0x02

#define ServoSensorPort S1
const byte ServoAddr = 0xb0;

int Servo1;

int Pos_Servo1;

int Servo1Speed;

task main ()
{
     psp currState;
     Pos_Servo1 = 100;
     Servo1 = 1;
     while (true) {
           Servo1Speed = abs(currState.r_j_y);
           if (currState.r_j_y > 0) {
           NXTServo_SetSpeed(SensorPort, ServoAddr, Servo1, Servo1Speed);
           NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, Servo1, Pos_Servo1);
           Pos_Servo1 += 2;
           Wait (100);
           }
           if (currState.r_j_y < 0) {
           NXTServo_SetSpeed(SensorPort, ServoAddr, Servo1, Servo1Speed);
           NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, Servo1, Pos_Servo1);
           Pos_Servo1 -= 2;
           Wait (100);
           }
     }
}

[emanuellinux]

I finally found out what was wrong with the program. I was using SensorPort instead of ServoSensorPort in the NXTServo_Quick_Servo_Setup parameters. On the other hand I ran into a different problem. Sometimes the servo will jump to the other side instead of stopping. Any ideas on how to fix this?

Thanks

[afanofosc]

It turns out that I did already implement functions in the NXC API for both the PSP device and the NXTServo device. My apologies for not remembering that correctly.

http://bricxcc.sourceforge.net/nbc/nxcd ... ample.html

http://bricxcc.sourceforge.net/nbc/nxcd ... ample.html

These API functions may work for you. For certain, if you have troubles with them I'll be able to help you. I do have both devices so I could try out code that you are having problems getting to work.

John Hansen

[afanofosc]

It would help if I knew what you were trying to accomplish with the combination of NXTServo and PSP devices. In your simple program it looks like you want the right Y joystick value (which will range from -127 to 128) to determine the speed that the servo uses to move + or - 2 "degrees" from its current position each 100 ms. I'm not sure that makes sense - and you may need to limit the speed value to something less than the absolute value of the right Y joystick value.

Here's some code that uses my API functions:

Code: Select all

#define ServoPort S1
#define PSPPort S2

task main ()
{
     byte btnset1, btnset2;
     char xleft, yleft, xright, yright;
     int Pos_Servo1;
     int Servo1Speed;

     SetSensorLowspeed(PSPPort);
     SetSensorLowspeed(ServoPort);

     Pos_Servo1 = 100;
     while (true) {
           ReadSensorMSPlayStation(PSPPort, MS_ADDR_PSPNX, btnset1, btnset2, xleft, yleft, xright, yright);
           Servo1Speed = abs(yright);
           SetNXTServoSpeed(ServoPort, MS_ADDR_NXTSERVO, NXTSERVO_SERVO_1, Servo1Speed);
           if (yright > 0) {
             Pos_Servo1 += 2;
           }
           else if (yright < 0) {
             Pos_Servo1 -= 2;
           }
           SetNXTServoQuickPosition(ServoPort, MS_ADDR_NXTSERVO, NXTSERVO_SERVO_1, Pos_Servo1);
           Wait (100);
     }
}

John Hansen

[emanuellinux]

It would help if I knew what you were trying to accomplish with the combination of NXTServo and PSP devices. In your simple program it looks like you want the right Y joystick value (which will range from -127 to 128) to determine the speed that the servo uses to move + or - 2 "degrees" from its current position each 100 ms. I'm not sure that makes sense - and you may need to limit the speed value to something less than the absolute value of the right Y joystick value.

Here's some code that uses my API functions:

|code|

John Hansen

I would like the program to function something like what you mentioned. Basically I would like to have the servo move according to how far the joystick is pushed in either direction. If the joystick is pushed slightly I would like the servo to move at a slow pace until the joystick is returned to a value of 0 (or until the servo reaches it's prospective max, which the servos I currently use have 180 degrees of motion), but if the joystick is pushed all the way in a direction then I would like the servo to move faster until the joystick is returned to the 0 value (or the servo reaches it's max). In other words, servo speed dynamically reacts to joystick value. I thought the value for the joystick was between -100 and 100, as the NXC sample program that returns values on the screen varies between that.
The program I wrote, with some modification (like removing wait times) worked, though it didn't work to it's potential. I will test your code either later tonight or tomorrow, depending on if I have time.

Thank you