Chief Delphi - Speed limiting code from last year in C?

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - Speed limiting code from last year in C? (http://www.chiefdelphi.com/forums/showthread.php?t=27271)

Speed limiting code from last year in C?

I've been working on our EduBot programming all week, and I haven't been able to successfully port over the speed-limiting code from last year (jumper on SW7) to C. I'm trying to cut speed for debug purposes with the variable 'topspeed' (0-254), and also have a table of volts->topspeed variable to mantain speed for DR programs, etc. Sadly, even if it compiles, it makes one of the motors just pulse on and off and the other continues at full speed.

I wish I could post all the ways I tried, I think I still have two snippets left, but I thought it would be best to ask if anyone here has done/knows how to make this code fuction without having to deal with my crappy, innefficient ways 'n such.

I can post my code without the limiting section if that will help anyone.

pwm01 is left motor drive, pwm02 is right motor drive. Thanks so much! :)

Re: Speed limiting code from last year in C?

I'm not sure I understand what you're trying to do, but a couple things:

If you are just trying to limit the output to the motors (pwm values), you can use a MIN_MAX macro (essentially a double terniary operator), or MIN/MAX depending on what you are doing.

#define MIN(value, min) (value < min ? min : value)
(or, if value is less than min, min, otherwise value)
and
#define MIN_MAX(value, min, max) (value < min ? min : (value > max ? max : value))
(same thing, but checks max after min).
edit: careful about using these, as terniary operators take up lots of program space!

I suppose if you are trying to limit the actual speed of the wheels, you are going to need some sort of sensory input, and then decrease the value of the motors as you go. That takes a bit more doing.

Sorry if I totally misunderstood what ya meant :).

Re: Speed limiting code from last year in C?

Well, I just spent 18 minutes re-writing the code on here (my laptop isn't with me) and it didn't post! :mad:

I'm sorry, I'm exhausted, and I'm going to bed now, but here's what I'm trying to do, with topspeed instead of p1_wheel:

p1_wheel = (((p1_wheel*154)/254)+100) MAX 254 'adjust wheel to 154-254
IF drive_R < 127 THEN drive_R_reverse: 'is the right drive forward
drive_R = (drive_R - 127) MIN 0 'subract 127 to get the forward value
drive_R = (drive_R * p1_wheel)/254 'multiply by the wheel percentage
drive_R = (drive_R + 127) MAX 254 'add 127 back for proper output
GOTO drive_R_done: 'exit the drive right section
drive_R_reverse: 'the right drive is reverse
drive_R = (127 - drive_R) MIN 0 'invert drive-R to get a forward value
drive_R = (drive_R * p1_wheel)/254 'multiply by the wheel percentage
drive_R = (127 - drive_R) MIN 0 'invert drive_R back to normal
drive_R_done: 'drive_R section complete

IF drive_L < 127 THEN drive_L_reverse: 'is the left drive forward
drive_L = (drive_L - 127) MIN 0 'subract 127 to get the forward value
drive_L = (drive_L * p1_wheel)/254 'multiply by the wheel percentage
drive_L = (drive_L + 127) MAX 254 'add 127 back for proper output
GOTO drive_L_done: 'exit the left right section
drive_L_reverse: 'the left drive is reverse
drive_L = (127 - drive_L) MIN 0 'invert drive-L to get a forward value
drive_L = (drive_L * p1_wheel)/254 'multiply by the wheel percentage
drive_L = (127 - drive_L) MIN 0 'invert drive_R back to normal
drive_L_done: 'drive_L section complete

Edit: Okay, so I lied, I'm not going to bed just yet :P. I have to get this programming bug out of my system!;

Does this look like it would work? Its should be the same as above, except without the error protection (min 0/max 254)

Code:

unsigned int topspeed = 127;



if (pwm01 > 127) {

pwm01 = ((((pwm01 - 127)*topspeed)/254)+127);

}

if (pwm02 > 127) {

pwm02 = ((((pwm02 - 127)*topspeed)/254)+127);

}

if (pwm01 < 127) {

pwm01 = (127-(((127 - pwm01)*topspeed)/254));

}

if (pwm02 < 127) {

pwm02 = (127-(((127 - pwm02)*topspeed)/254));

}

Re: Speed limiting code from last year in C?

Our coder came up with a thing that may help (I hope)
this is in the header file:

#define Grabber_Speed p4_x /*port 4 x on OI */
#define Forward_Speed (127 + (Grabber_Speed / 2))
#define Reverse_Speed (127 - (Grabber_Speed / 2))

The Grabber_Speed is linked to a Knob (pot) on the IO

We are now just using IO switches to go Forward and Reverse So forward and reverse get pluged in with if statements.
The Range for Grabber_Speed is 0 to 254 as it comes from The OI

Re: Speed limiting code from last year in C?

Looks good, though I would suggest you take advantage of signed math in the following way.

Code:

 int tempPwm01;

 int topSpeed = 127;

 

 tempPwm01 = (int)pwm01 - 127; //0 center [-127 to 127]

 tempPwm01 = tempPwm01 * topSpeed / 254; //Reduce range to

                                         //[-topSpeed / 2 to topSpeed / 2]

 pwm01 = tempPwm01 + 127; //127 center

I did wrote it spread out like that to make it easier to understand, but it can all be reduced to 1 statement:

Code:

pwm01 = 127 + ( ((int)pwm01 - 127) * topSpeed / 254 );

As long as you are sure topSpeed will never exceed 254 and will never go below 0 you don't need to min/max anything.

By the way, just to confirm that this is what you are trying to achieve. The above code will turn a pwm value of 254 into 127 + (topSpeed / 2) and a pwm value of 0 into 127 - (topSpeed / 2). It's the same as the C code that you posted but I just want to make sure that's what you want.

Re: Speed limiting code from last year in C?

Yep, thats what I wanted to do! Thanks! I'll go test it out this morning :).

Edit: Well, I'm looking at my code (I'll post a copy soon for anyone who wants), and I'm not quite sure where this should go/how to call it. I was thinking of rather than

goto mainloop;

i'd say

goto topspeed_code;

then in topspeed_code, call 'mainprogram.'

That makes sense to me, BUT - I have timed loops for turning so I can't call the routine while it's excecuting - do i put a copy in the same loop with the turning values?

Code:

/*

Author: Joshua Siegel

Revision: 3-28-04, 4:54PM

Purpose: Go through a maze and find and extinguish a candle in the middle of a 15cm-radius white circle on black wood. Return trip optional, not attempted in this version.

What it's supposed to do:

1. If UV is false, go to maze logic (can explain if desired, but its pretty simple).

2. If UV is true, count the number of loops spent on white lines on a black floor. We need to stop on a white circle (filled in), so the counter resets itself to avoid mistaking the line for the circle.

4. If UV is true and the robot is not on the circle, go right for 21 loops, then let it resort to the default action until the white line reads true again. This lets the sensor correct for the candle being on the right, but it will find it even if it is on the left because a left turn is the default action.

5. If UV is true and the robot is on the circle, stop and turn on a fan.

6. Not quite sure what to do if the robot is on the circle, but no UV is present (I was thinking of going to findright?).

Sensors: rc_dig_in01 = front IR, rc_dig_in02 = left IR, rc_dig_in03 = right IR, rc_dig_in_04 = UV detector, rc_dig_in05 is the bottom/white line/circle sensor).

To be added:  Speed/voltage function if time is left (the old PBASIC robot had a feature where topspeed was limited and as voltage dropped, the topspeed increased so that physical speed remained the same. This made it much easier to program). Return trip.

Notes: Straight has been replaced with soft left (I realized that in open rooms and not hallways, an all-false situation becomes very likely). Disconnect UV sensor power or 9V's entirely, or else breakers will trip even during 'off' mode. Set sensors up to wall color, not hands, paper, or other objects! White value greatly affects read distance. Also, I tried a right-pulse counter to aid in turning. Same with UV (to help assure there is a real flame). Maze logic over-rides candle if IR = 0 for any of the three sensors. 

Known issues: Floorcount doesnt seem to work yet.

Accuracy of current revision: 13/15 coming from right. 0/1 coming from left.

*/

#include "ifi_aliases.h"

#include "ifi_default.h"

#include "ifi_utilities.h"

#include "user_routines.h"

#include "printf_lib.h"

/* What about 'until logic?' at a later date? - If left true, front false, turn soft right until right true?*/

/* Hard right until left true, hard left until right true. soft left until right true, soft right until left true. */

unsigned int counter = 0;

unsigned int rightturntime = 350;        

unsigned int leftturntime = 150;

unsigned int countloop = 0;

unsigned int countloop2 = 0;

unsigned int whitepulse = 0;

unsigned int foundUV = 0;

unsigned int howmanyright = 0;

unsigned int UVpulsecount = 0;

unsigned int UVpulsetimer = 0;

unsigned int UVread = 1;

static void Setup_Who_Controls_Pwms(int pwmSpec1,int pwmSpec2,int pwmSpec3,int pwmSpec4,int pwmSpec5,int pwmSpec6,int pwmSpec7,int pwmSpec8)

{

  txdata.pwm_mask = 0xFF;

  if (pwmSpec1 == USER)

    txdata.pwm_mask &= 0xFE;

  if (pwmSpec2 == USER)

    txdata.pwm_mask &= 0xFD;

  if (pwmSpec3 == USER)

    txdata.pwm_mask &= 0xFB;

  if (pwmSpec4 == USER)

    txdata.pwm_mask &= 0xF7;

  if (pwmSpec5 == USER)

    txdata.pwm_mask &= 0xEF;

  if (pwmSpec6 == USER)

    txdata.pwm_mask &= 0xDF;

  if (pwmSpec7 == USER)

    txdata.pwm_mask &= 0xBF;

  if (pwmSpec8 == USER)

    txdata.pwm_mask &= 0x7F;

}

void User_Initialization (void)

{

  IO1 = IO2 = IO3 = IO4 = IO5 = INPUT;

  Set_Number_of_Analog_Channels(TWO_ANALOG);

  rc_dig_out06 = 1;

  rc_dig_out07 = rc_dig_out09 = 0;

  rc_dig_out11 = rc_dig_out13 = rc_dig_out15 = 0;

  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;

  Setup_Who_Controls_Pwms(MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER);

  Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);

  Initialize_Serial_Comms();     

  Putdata(&txdata);

  User_Proc_Is_Ready();

}

void Process_Data_From_Master_uP(void)

{

  Getdata(&rxdata);

  Default_Routine();

  Putdata(&txdata);

}

void Default_Routine(void)

{

mainprogram:

Putdata(&txdata);

foundUV = 0;

printf ("Floor: %d\n", (int)rc_dig_in05);

UVpulsetimer++;

countloop++;

countloop2++;

if (rc_dig_in04 == 1) {

UVpulsecount++;

}

if (UVpulsetimer >= 30) {

        UVpulsetimer = 0;

        if (UVpulsecount >= 4) {

        UVpulsecount = 0;

        UVread = 0;

        }

        else if (UVpulsecount < 4) {

        UVpulsecount = 0;

        UVread = 1;

        }

}

if (rc_dig_in05 == 0) {

whitepulse++;

}

if (rc_dig_in02 == 0) {

howmanyright++;

/*Which, come to think of it, is actually the left sensor! It is called howmanyright because it tries to deter>=e if the turn co>=g up should be right or left.*/

}

if (countloop >= 450) {

        countloop = 0;

        whitepulse = 0;

        goto mainprogram;

}

if (countloop2 >= 1150) {

countloop2 = 0;

howmanyright = 0;

}

if (UVread == 1) {

        goto logic;

}

else if (UVread == 0) {

        if (whitepulse <= 100) {

                if (foundUV <=25) {

                findright:

                pwm01 = 254;

                pwm02 = 245;

                Putdata(&txdata);

                foundUV++;

                }

                if (foundUV >=26) {

                goto mainprogram;

                }

        goto mainprogram;

        }

        if (whitepulse >= 101 && UVread == 0) {

        pwm01 = 127;

        pwm02 = 127;

        rc_dig_out06 = 0;

        }

        if (whitepulse >= 101 && UVread == 1) {

        goto findright;

        }

}

logic:

Putdata(&txdata);

pwm01 = 240;

pwm02 = 1;

if (rc_dig_in01 == 0)  {

        goto Front_True;

}

if (rc_dig_in01 == 1) {

        goto Front_False;

}

Front_True:

        if (rc_dig_in02 == 0) {

        goto FrontTrueLeftTrue;

        }

        if (rc_dig_in02 == 1) {

        goto FrontTrueLeftFalse;

        }

goto mainprogram;

Front_False:

        if (rc_dig_in02 == 0) {

        goto FrontFalseLeftTrue;

        }

        if (rc_dig_in02 == 1) {

        goto FrontFalseLeftFalse;

        }

goto mainprogram;

FrontTrueLeftTrue:

        if (rc_dig_in03 == 0) {

        goto BackUp;

        }

        if (rc_dig_in03 == 1) {

        goto HardRight;

goto mainprogram;

FrontTrueLeftFalse:

        if (rc_dig_in03 == 0) {

        goto HardLeft;

        }

        if (rc_dig_in03 == 1 && howmanyright <= 0) {

        goto HardLeft;

        }

        if (rc_dig_in03 == 1 && howmanyright > 0) {

        goto HardRight;

        }

goto mainprogram;

FrontFalseLeftTrue:

        if (rc_dig_in03 == 0) {

        goto Straight;

        }

        if (rc_dig_in03 == 1) {

        goto SoftRight;

        }

goto mainprogram;

FrontFalseLeftFalse:

        if (rc_dig_in03 == 0) {

        goto SoftLeft;

        }

        if (rc_dig_in03 == 1) {

        goto SoftLeft; /*Was straight, then changed to soft left in a mad fit of logic)*/

        }

goto mainprogram;

/*pwm01 is reversed*/

BackUp:

  printf("Mode: Backing up\n");

pwm01 = 1;

pwm02 = 254;

goto mainprogram;

HardRight:

counter++;

if (counter <= rightturntime) {

        pwm01 = 254;

        pwm02 = 210;

        Putdata(&txdata);

        printf("Mode: Hard right\n");

        goto HardRight; 

        }

else if (counter > rightturntime) {

        counter = 0;

        goto mainprogram;

}

goto mainprogram;

HardLeft:

counter++;

if (counter <= leftturntime) {

        printf("Mode: Hard left\n");

        pwm01 = 1;

        pwm02 = 1;

        Putdata(&txdata);

        goto HardLeft;

        }

        else if (counter > leftturntime)

        counter = 0;

        goto mainprogram;

        }

goto mainprogram;

Straight: /*Straight is still used in left true/right true/front false situations.*/

printf("Mode: Straight\n");

pwm01 = 240;

pwm02 = 1;

goto mainprogram;

SoftLeft:

printf("Mode: Soft left\n");

pwm01 = 240;

pwm02 = 1;

goto mainprogram;

SoftRight:

printf("Mode: Soft right\n");

pwm01 = 254; /*254*/

pwm02 = 20; /*20*/

goto mainprogram;

}

Re: Speed limiting code from last year in C?

*bump!*

Well, I know what needs to be done now. I can't limit the speed of timed turns, so I need to use pwm01 for those and temppwm01 for the values that change. Which would be better, trying to add the code to the putdata (I don't know how to edit a .lib?), or putting it at the start of the loop? The problem I can forsee with that is that I have multiple putdatas, some in loops and some not, and to loop the topspeed code wouldn't be clean or efficient.