Connecting Shadow-Bot's DOF's

Gramps

Senior Member
Buzby and Ibenson cracked the BNO055 9 DOF Absolute Orientation IMU.
We finally got (thank you Allycat and Ibenson) a correctly operating MD20A motor controller code.
Servo code looks like it's cut and paste from the manual. (probably isn't!:rolleyes:)
Now to tie it all together.
We want the BNO055 to handle the lower shoulder, upper shoulder, and the rotator cuff.
The Lower shoulder and upper shoulder are large MD20A motors.
The rotator cuff is a hefty servo.
Thinking that a Picaxe 28X2 mounted just below the BN0055 would fill the bill.

The elbow is also a MD20A motor.
The wrist is a servo.
Fingers and thumb are two separate servos. Hopefully opening and closing them together with one pot. Later down the road add some independent finger motors.
So what do you guys think?
A 28x2 or a 40x2 to handle servo and motor code?
I can see now how important it will be to have orderly code that is well documented and symbolled!!!
 

Gramps

Senior Member
Another thought was to use a separate 08M2 for each individual joint not controlled by the BN0055.
Edit: That would be a stretch for an 08M2!!!
 

Gramps

Senior Member
use a separate 08M2 for each individual joint
I'm away from the hardware this week but it looks like the Shadow-Bot feedback control code will run on a 08M2 chip! (works in the simulator.)
Below is the updated code.
Code:
'Shadow-Bot feedback control code with limits for 08M2
#picaxe 08M2
#no_data
'#no_table
'C.1 = ADC Control pot
'C.2 = Motor
'C.4 = ADC feedback pot
'C.0 = Direction change

symbol DEADZONE = 4   ; Switch motor off (Make as small as practical)
symbol SLOWZONE = 12  ; Slow motor down (NB: May depend on motor speed ! )
symbol MotorSpeed  = w4  ; (To replace the "300" in the present PWMOUT code)
symbol LOOPGAIN = 33      ;  i.e.  MAXPWMVALUE / SLOWZONE (= 399 / 12)
symbol PWMPERIOD = 199   ; Equivalent Max Duty Cycle is 799
symbol xb0 = b0 ' reserved for bit variables
symbol bReverseForward=bit1 ' 0=reverse,=forward
symbol bMotorState=bit0     ' 0=off,1=on
symbol REVERSE_=0
symbol FORWARD_=1
Symbol desired_pot_value = b1
Symbol feedback_pot_value = b3
Symbol diff=b4
Symbol  desired_pot = C.1 'control pot
Symbol  feedback_pot = C.4
Symbol MOTOR = C.2' Energize PWM
Symbol Direction = C.0
Symbol MAXPWMVALUE = 399
Symbol bSlowState=bit2
Symbol old_feedback_value= b5
Symbol old_desired_value= b6
Symbol ENDSTOP_A  = 35 ; desired_pot MINimum value
Symbol ENDSTOP_C = 210 ; desired_pot MAXimum value
;Lower shoulder min=35 max=210
;Upper shoulder min=?? max=???

Main:
  Readadc Desired_pot, desired_pot_value
    Desired_pot_value = Desired_pot_value MIN ENDSTOP_A MAX ENDSTOP_C
    'sertxd (#desired_pot_value," ",#b1,13,10)
    Readadc feedback_pot, feedback_pot_value
   ' sertxd (#feedback_pot_value," ",#b3,13,10)
  'sertxd("Desired/Feedback: ",#desired_pot_value," ",#feedback_pot_value,13,10)
  If old_feedback_value<>feedback_pot_value or old_desired_value<> desired_pot_value then
  old_feedback_value=feedback_pot_value
  old_desired_value=desired_pot_value
  sertxd("Desired/Feedback: ",#desired_pot_value," ",#feedback_pot_value,13,10)
endif
if desired_pot_value > feedback_pot_value then
   diff=desired_pot_value - feedback_pot_value
   else
   diff=feedback_pot_value - desired_pot_value
endif

  if diff > DEADZONE then
    MotorSpeed = diff * LOOPGAIN max MAXPWMVALUE  ; Limits speed outside of SLOWZONE
    if  desired_pot_value > feedback_pot_value then
      high Direction       ; Set forward direction
    else
      low Direction        ; Set reverse direction
    endif
    pwmout MOTOR , PWMPERIOD , MotorSpeed     ; Slows motor within SLOWZONE
  else
    pwmout MOTOR , OFF         ; STOP
    bMotorState=0      ; Not sure what these do (now)
    bSlowState = 0      ;
  endif

goto main
 

lbenson

Senior Member
Note that you cannot use SERTXD (which outputs to pin C.0) if you are using C.0 for the direction. Maybe time for a 14M2?
 

Gramps

Senior Member
you cannot use SERTXD
Correct.
It should be possible to get the code running smoothly with the hardware and then load it to the 08M2, eliminating SERTXD and unplugging the serial cable.
 
Last edited:

Gramps

Senior Member
Attempting to combine the code reading the BNO055 IMU with the MD20A motor controller.
Please walk me (very slowly) through this transition.
We want to send the data produced by "BNO_YAW = W5" to become the "desired_pot_value = b1"
Correct?
Code:
' Combining MD20A with BNO055 IMU
#Picaxe 28X2
#NO_table

    Symbol xb0 = b0 ' reserved for bit variables
    Symbol bReverseForward=bit1 ' 0=reverse,=forward
    Symbol bMotorState=bit0     ' 0=off,1=on
    Symbol REVERSE_=0
    Symbol FORWARD_=1
    Symbol desired_pot_value = b1
    Symbol feedback_pot_value = b3
    Symbol diff=b4
    Symbol  desired_pot = 13 'B.5
    Symbol  feedback_pot = 11'B.4
    Symbol MOTOR = B.0' Energize PWM
    Symbol Direction = B.7
    Symbol MAXPWMVALUE = 399
    Symbol bSlowState=bit2
    Symbol old_feedback_value= b5
    Symbol old_desired_value= b6
    Symbol ENDSTOP_A  = 35 ; desired_pot MINimum value
    Symbol ENDSTOP_C = 210 ; desired_pot MAXimum value
    Symbol BNO_YAW   = W5
    Symbol BNO_PITCH = W6
    Symbol BNO_ROLL  = W7
    
    pause 2000
sertxd (ppp_filename," ", ppp_datetime,cr,lf)
 
    HI2cSetup i2cmaster, $50, i2cslow, i2cbyte; set PICAXE as master and BNO055 slave address
    Pause 1000    
main:    hi2cin 0,(b0,b1,b2,b3,b4,b5,b6)    ; read data and debug display    
     
    debug b1
    pause 500
    hi2cin $1A,(b10,b11)  ;heading ( yaw )$1A
    hi2cOut $3D, ($0C)
    hi2cin $1E,(b12,b13) ;pitch 1F
    hi2cin $1D,(b14,b15) ;roll $1C
    ;SERTXD(#w5,cr,lf)

   Readadc Desired_pot, desired_pot_value
    Desired_pot_value = Desired_pot_value MIN ENDSTOP_A MAX ENDSTOP_C
    'sertxd (#desired_pot_value," ",#b1,13,10)
    Readadc feedback_pot, feedback_pot_value
   ' sertxd (#feedback_pot_value," ",#b3,13,10)
  'sertxd("Desired/Feedback: ",#desired_pot_value," ",#feedback_pot_value,13,10)
  If old_feedback_value<>feedback_pot_value or old_desired_value<> desired_pot_value then
  old_feedback_value=feedback_pot_value
  old_desired_value=desired_pot_value
  sertxd("Desired/Feedback: ",#desired_pot_value," ",#feedback_pot_value,13,10) 
endif
if desired_pot_value > feedback_pot_value then
   diff=desired_pot_value - feedback_pot_value
   else
   diff=feedback_pot_value - desired_pot_value
endif
if diff < 12 then ' from 11 below a match to 11 above
  if bMotorState=1 then
    if diff > 4 then ' this value may need to be set experimentally
      if bSlowState = 0 then
        bSlowState = 1 ' note that we are in the "slow speed" state
        pwmout MOTOR, 199, 300 ' slow down to half speed
      endif
    else
      pwmout MOTOR, OFF ' STOP
      bMotorState=0
      bSlowState = 0
    endif
  endif
else ' motor must be set for proper direction and then activated
  if  desired_pot_value > feedback_pot_value then
    if bReverseForward = REVERSE_ then
      high Direction ' go forward
      bReverseForward = FORWARD_
    endif
  elseif  desired_pot_value < feedback_pot_value then
    if bReverseForward = FORWARD_ then
      low Direction ' go in reverse
      bReverseForward = REVERSE_
  endif
  endif
  if bMotorState=0 then
    bMotorState=1
    pwmout MOTOR, 199, MAXPWMVALUE' rotate Reverse
  endif
endif

goto main
 

Gramps

Senior Member
We want to send the data produced by "BNO_YAW = W5" to become the "desired_pot_value = b1"
I'm thinking the Yaw value must be mathematically converted to the value that the desired_pot_value is expecting to see.
And that since
Symbol ENDSTOP_A = 35 ; desired_pot MINimum value
Symbol ENDSTOP_C = 210 ; desired_pot MAXimum value
it will have to be NOT less then 35 or MORE then 210.
Would it be better to put the endstops on the feedback pot?
 

lbenson

Senior Member
Would it be better to put the endstops on the feedback pot?
Better than what?

Allycat previously showed how to do that with MIN and MAX, but what is the range of values from BNO_YAW that you wish to convert to 35 to 210?
That will provide the ratio you have to use in the conversion.
 

Gramps

Senior Member
Better than what?
Sorry. Better to endstop the feedback pot then having them on the desired pot.
Do we even need a desired pot in this application?
what is the range of values from BNO_YAW
Checking.....
Cannot get the sensor to calibrate!!
With poor calibration it's 25 to 115
Let's go with these numbers so I can see how the math is done...
 
Last edited:

PieM

Senior Member
To convert BNO_Yaw from 25 - 115 to 35 - 210 :

Y = 175 * BNO / 90 - 14

Line equation is: Out = a * BNO + b
a = (out2- out1) / (BNO2 - BNO1)
b = out2 - (a* BNO2)
 
Last edited:

AllyCat

Senior Member
Hi,
Please advise where this is discussed in the manual
It's not really a PICaxe issue, but basic Maths, so I wouldn't expect to find it in a PICaxe Manual. But it has been discussed many times on the forum, by hippy for example, in the context of mapping ADC values (e.g. 0 up to 255) to the sweep range of a Servo (e.g. 75 up to 225).

The method is basically to subtract the "sit up", (i.e. the minimum value) of the input value (e.g. b1 = b1 - 25 ) then scale the gradient (i.e. the range) of the line (e.g. b1 = b1 * 175 / 90 ) where 210 - 35 = 175 and 115 - 25 = 90 , then add in the new sit-up, (e.g. b1 = b1 + 35 ).
____

You can simply apply MIN and MAX endstops to any input value, not just from the "desired" Pot. But note that you must NOT use MIN and MAX for the feedback signal (because it could overload the motor). There you must use e.g. IF feedback_value < ENDSTOP_A OR IF feedback_value > ENDSTOP_C THEN GOTO stop . In principle, you can use either method (but IMHO the MIN / MAX is simpler), provided that the "desired" and "feedback" values are equivalent. That's why I insisted way back (in the other thread) that the motor should be designed to stop when the feedback_value = desired_value exactly (not just "give or take 4").

Cheers, Alan.
 

AllyCat

Senior Member
Hi,

Substitute whichever variable you're using for b1 and it can be written in a single line:
b1 = b1 - 25 * 175 / 90 + 35
Or exactly as I wrote above, or with a couple of limits set to trap any underflow or overflow, in case the input data is out of range:
Code:
b1 = b1 MIN 25 - 25     ; Move minimum to zero, avoiding underflow
b1 = b1 * 175 / 90      ; Change Scale
b1 = b1 + 35 MAX 210    ; Set maximum, avoiding overflow
or still written as a one-liner: b1 = b1 min 25 - 25 * 175 / 90 + 35 max 210

Cheers, Alan.
 

Gramps

Senior Member
This is getting pretty discouraging.
For some reason we're getting a high number on Yaw to the left of zero.
It starts 5758 and drops down to 4400.
Right of center it counts from 0 to 1200.
Pitch and Roll are reading correctly.


24234

Here is the code;
Code:
;Buzby'sV.2 BNO055 DOF code to read sensor;
; SCL connects to C.3
;SDA connects to C.4
Code:
#picaxe 28X2
#no_table
#no_data

symbol BNO_YAW   = w5
symbol BNO_PITCH = w6
symbol BNO_ROLL  = w7

symbol P_sgn     = b8
symbol R_sgn    = b9

sertxd(cr,lf,ppp_filename," Starting ...",cr,lf)
pause 1000
  
' Setup I2C 
hi2cSetup i2cmaster, $50, i2cslow, i2cbyte

' Set BNO active
hi2cOut $3D, ($0C)

do   
   ' Read chip ID     
   hi2cin 0,(b0)  
  
   ' Quit if chip ID wrong
   if b0 <> $A0 then : sertxd( "Bad comms !",cr,lf): reset : endif
  
   ' Read Euler angles
   hi2cin $1A,(b10,b11)  ' Yaw,        0 to +360
   hi2cin $1C,(b12,b13)  ' Pitch,   -180 to +180
   hi2cin $1E,(b14,b15)  ' Roll,     -90 to +90
  
   ' Convert pitch to +/- value
   if BNO_PITCH > $8000 then
      P_sgn = "-"
    BNO_PITCH = 0 - BNO_PITCH
   else
    P_sgn = "+"
   endif   

   ' Convert roll to +/- value
   if BNO_ROLL > $8000 then
      R_sgn = "-"
    BNO_ROLL = 0 - BNO_ROLL
   else
    R_sgn = "+"
   endif   

   ' Show results
   sertxd("Y=",#BNO_YAW,9,9,"P=",P_sgn,#BNO_PITCH,9,9,"R=",R_sgn,#BNO_ROLL,cr,lf)
  
   pause 200
  
loop


rem So now if you rotate the BNO flat on the table, Yaw should change.
rem If you tilt it forward/backward, Pitch should change.
rem If you tilt it left/right, Roll should change.
 

lbenson

Senior Member
5758 divided by 16 is 359.875, which is suspiciously close to 360. 4400/16 is 275--close to max minus 90 degrees. I don't remember all of the foregoing, but are you forgetting to divide by 16?

And I vaguely remember something about YAW not distinguishing between forward and backward. (And is this relative to north?) If flat on the table you rotate clockwise 360 degrees starting from north, what readings do you get (dividing yaw by 16).

Also, so you don't flood the SERTXD output, try it dividing by 160 and only printing the output if YAW10 differs from lastYAW10 (word variables you must declare and calculate and save each time there is a change after that division).
 

PieM

Senior Member
Hi,
Yaw, pitch and roll are in 1/16 degree.
Yaw is 0 to 360 if you rotate clockwise from north
24236
 
Last edited:

Gramps

Senior Member
Sorry guys. The code in post 16 is an old save.
Here is the working code .
Code:
picaxe 28X2
#no_table
#no_data

' BNO055 Test - V3
' -----------------

 symbol BNO_YAW   = w5    ' b10, b11
 symbol BNO_ROLL  = w6    ' b12, b13
 symbol BNO_PITCH = w7    ' b14, b15
 
 symbol CalState  = b4
 
 symbol P_sgn     = b8
 symbol R_sgn    = b9

' Code starts here

 sertxd(cr,lf,ppp_filename," Starting ...",cr,lf)
 pause 1000
   
 ' Setup I2C  
 hi2cSetup i2cmaster, $50, i2cslow, i2cbyte
 
 ' Set BNO active
 hi2cOut $3D, ($0C)

do    
   ' Read chip ID      
   hi2cin $00,(b0)   
   
   ' Quit if chip ID wrong
   if b0 <> $A0 then : sertxd( "Bad comms !",cr,lf): reset : endif
   
   ' Read calibration state
   hi2cin $35,(CalState)   
   
   ' Read Euler angles
   hi2cin $1A,(b10,b11)  ' Yaw,        0 to +360  
   hi2cin $1C,(b12,b13)  ' Roll,     -90 to +90
   hi2cin $1E,(b14,b15)  ' Pitch,   -180 to +180
 
   ' Scale Yaw to 360'
   BNO_YAW = BNO_YAW / 16
 
   
   ' Convert pitch to +/- value
   if BNO_PITCH > $8000 then
      P_sgn = "-"
    BNO_PITCH = 0 - BNO_PITCH
   else
    P_sgn = "+"
   endif    
   ' Scale Pitch to 180'
   BNO_PITCH =  BNO_PITCH / 16 ' ?? might need changing


   ' Convert roll to +/- value
   if BNO_ROLL > $8000 then
      R_sgn = "-"
    BNO_ROLL = 0 - BNO_ROLL
   else
    R_sgn = "+"
   endif    
   ' Scale Roll to 90'
   BNO_ROLL =  BNO_ROLL / 16 ' ?? might need changing


   ' Show results
   sertxd("Y=",#BNO_YAW,9,9,"P=",P_sgn,#BNO_PITCH,9,9,"R=",R_sgn,#BNO_ROLL,9,9)
   
   ' Show calibration state
   if CalState = 255 then : sertxd( "Cal OK") : else : sertxd( "Cal poor") : endif
   
   ' New line
   sertxd(cr,lf)
   
   pause 200
   
loop
[\code][ATTACH type="full" alt="24237"]24237[/ATTACH]
 

Attachments

lbenson

Senior Member
Not sure what your screen shot is telling us. Is the problem in the previous post resolved?
 
Last edited:
Top