Subject:
|
Re: balancing bot
|
Newsgroups:
|
lugnet.robotics
|
Date:
|
Fri, 31 Dec 2004 04:58:02 GMT
|
Viewed:
|
3986 times
|
| |
|
|
I recently implemented a new motor control "source" specifically
targeted at PID algorithms in the new firmware. It takes a signed number
in the range -128 to +128 and converts it into a motor control command.
-128 to -1 is reverse direction and power level.
Zero is braked.
+1 to +128 is forward and power level.
This will replace the code in Philo's example from:
MotPwr = (MotCmd > 0) ? MotCmd : -MotCmd;
if(MotPwr > 127) MotPwr = 127;
motorPower128(Right_Pwr) = MotPwr;
motorPower128(Left_Pwr) = MotPwr;
if(MotCmd > 0) OnFwd (Right_M + Left_M);
else OnRev (Right_M + Left_M);
to:
motorPowerSigned(Right_M) = MotCmd ;
motorPowerSigned(Left_M) = MotCmd ;
The NQC compiler generates 15 opcodes for Philo's code snippet which
gets replaced with 2 with this new source. The 15 opcodes include a
number of conditional branches so best/worst case is 9 to 11 opcodes per
loop. This should nearly cut the execution time of one loop in half.
Another small point is that the statement " if(MotPwr > 127) MotPwr =
127;" is now unnecessary. In the original release of enhanced firmware
if you provided an out of range power level, it could cause strange
results because I was only looking at the lower 7-bits of the value to
determine level. Firmware now looks at complete number and treats out of
range values as full power. The statement 'DispDbg=MotCmd;' could also
be eliminated to improve loop speed.
New general release of enhanced firmware in a couple of days.
"Philippe Hurbain" <philohome@free.fr> wrote in message
news:I9JtwF.1KGx@lugnet.com...
> >
> > You may have a look to YAL: http://news.lugnet.com/robotics/?n=22181
>
> ...and here is a NQC sample code that works on YAL, using Dick Swan's
> fast
> firmware.
>
> Try it on a clear color surface (not necessarily even, some patterns
> help
> stabilize position) with overhead diffuse lighting. YAL must be
> started balanced
> (sensor calibration).
>
> Philo
>
> -----------------------------------------------
> #include "swan.nqh"
>
> #define Front_S SENSOR_1
> #define Rear_S SENSOR_3
> #define Right_M OUT_A
> #define Right_Pwr 0
> #define Left_M OUT_C
> #define Left_Pwr 2
> #define a 15
> #define b 5
> #define c 5
>
> int Offset;
> int OldDif;
> int NewDif, MotCmd, OldMotCmd, MotPwr;
> int DispDbg;
>
> task main ()
> {
> SetUserDisplay (DispDbg,0);
>
> SetSensor (Front_S, SENSOR_LIGHT);
> SetSensor (Rear_S, SENSOR_LIGHT);
> SetSensorMode (Front_S, SENSOR_MODE_RAW);
> SetSensorMode (Rear_S, SENSOR_MODE_RAW);
>
> bFloatDuringInactiveMotorPWM = false;
> nMotorTransitionDelay = 0;
>
>
> Wait(10);
> Offset = Front_S - Rear_S;
> OldDif = 0;
>
> motorPower128(Right_Pwr) = 0;
> motorPower128(Left_Pwr) = 0;
>
> OnFwd (Right_M + Left_M);
> OldMotCmd = 1;
>
>
> while (true)
> {
> NewDif = Front_S - Rear_S - Offset;
> MotCmd = (a * NewDif + b * (NewDif - OldDif)) / c;
> OldDif = NewDif;
> DispDbg=MotCmd;
> MotPwr = (MotCmd > 0) ? MotCmd : -MotCmd;
> if(MotPwr > 127) MotPwr = 127;
> motorPower128(Right_Pwr) = MotPwr;
> motorPower128(Left_Pwr) = MotPwr;
> if(MotCmd > 0) OnFwd (Right_M + Left_M);
> else OnRev (Right_M + Left_M);
> }
> }
> --------------------------------------------------
|
|
Message is in Reply To:
 | | Re: balancing bot
|
| (...) ...and here is a NQC sample code that works on YAL, using Dick Swan's fast firmware. Try it on a clear color surface (not necessarily even, some patterns help stabilize position) with overhead diffuse lighting. YAL must be started balanced (...) (22 years ago, 30-Dec-04, to lugnet.robotics)
|
8 Messages in This Thread:
 
  
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
