.: Vinculum :. - Hexabot

So langsam frage ich mich eine LED zum Debugen reicht. Die geraden Bewegungen funktionieren schon ganz wunderbar in alle Richtung, nur bei den Drehungen gibt es im Moment noch Probleme. Vor allen welche die gar nicht auf das Programm zurück zu führen sein dürften. So kann ich z.B. die y-Bewegung der Fussspitze konstant auf 0 setzen und trotzdem dreht sich das Bein ein wenig in diese Richtung.

EDIT: Problem gelöst, wozu hat man denn 8 Kerne in seinem µC sollen die was tun für ihr Geld und die Berechnung parallel durchführen!

Hallo Hanno,

das mit der LED zum debuggen ist oft sehr wenig. Meine Idee momentan ist, praktisch alles via I2C anzusteuern und dann nur den Kontroller auszutauschen. Zum Debuggen ist der Kontroller dann der PC via USB I2C Adapter ;)

Habe aber dazu noch nicht so viele Erfahrungen...

Gruß
Georg

Ich habe noch ein i2c Display rumliegen aber bisher die Mühe gescheut das anzuschließen bzw. zu programmieren. Liegt vielleicht daran, weil es mit 5V betrieben wird, meine µC aber nur mit 3,3V

Irgendwo in den unendlichen Weiten meines Programmcodes habe ich mich verloren bzw. hatte es irgendwann soweit verbugged äh debugged, dass es gar nimmer recht synchron laufen wollte mit den Beinen. Jetzt allerdings bin ich endlich soweit, dass zumindest schon mal translatorisch vollständig funktioniert. Morgen werde ich mich dann noch um den rotatorischen Part kümmern und meine neuen Servos vom Zoll abholen. Dann könnte man endlich auch richtig Tests machen.

Aus alt macht neu. Einen der defekten Servos konnte ich mit einen anderen defekten Servo zu einem funktionierenden Servo umbauen. Jetzt hat das Vinculum wieder 6 funktionsfähige Beine und ich kann mich an die restliche Programmierung machen. Danach gibt es dann auch wieder ein Video vom aktuellen Stand der Technik.

Diesmal sind es nur 1050 Zeilen Code geworden. Mein Phoenix² ist noch mit etwa 800 Zeilen ausgekommen.

Code:

---

CON
  _CLKMODE = XTAL1 + PLL16X    'Set to ext crystal, 16x PLL, 80MHz Clock
  _XINFREQ = 5_000_000          'Frequency on XIN pin is 5 MHz 

  l_fuss    = 138.0            'Länge vom Fuss (b)     
  b_2        = 19044.0          ' (l_fuss)²             

  l_schenk  =  110.0          'Länge vom Oberschenkel 
  a_2        =  12100.0        ' (l_schenk)²
                                   
  aa_p_bb    =  31144.0        'a²+ b²    !!! mit xxx.0 angeben
  aa_m_bb    = -6944.0          'a²- b²    !!! mit xxx.0 angeben
  ax2        =  220.0          '2a        !!! mit xxx.0 angeben
  abx2      =  30360.0        '2ab        !!! mit xxx.0 angeben

  'Globale Werte für HEXA - können noch dynamisch gemacht werden
  x_off      =  101.0
  'Ri_max    =  100.0
  'Steps    =  11  -7>steps_all>7
  ON        = 1
  OFF        = 0

OBJ
  math    : "Float32Full" 
 
Var
    '-----------Servo-Puls-Erzeugung----------------------
  LONG new_peri_r,PinStart_r,pin_r,high_pulse_r[9]        'Reihenfolge nicht ändern da Pointer in Move-Servo darauf zu greifen
  LONG new_peri_l,PinStart_l,pin_l,high_pulse_l[9]        'DO NOT TOUCH
  LONG periode
  LONG stack_setpulse[25]

  LONG speed
  LONG Apos[3], Aini[3], Alast[3], Aziel[3], Asign[3]
  LONG Bpos[3], Bini[3], Blast[3], Bziel[3], Bsign[3] 
  LONG Cpos[3], Cini[3], Clast[3], Cziel[3], Csign[3]
  LONG Dpos[3], Dini[3], Dlast[3], Dziel[3], Dsign[3]
  LONG Epos[3], Eini[3], Elast[3], Eziel[3], Esign[3] 
  LONG Fpos[3], Fini[3], Flast[3], Fziel[3], Fsign[3] 
  LONG xA_trans, xB_trans, xC_trans, xD_trans, xE_trans, xF_trans
  LONG yA_trans, yB_trans, yC_trans, yD_trans, yE_trans, yF_trans
  LONG xA_rota,  xB_rota,  xC_rota,  xD_rota,  xE_rota,  xF_rota
  LONG yA_rota,  yB_rota,  yC_rota,  yD_rota,  yE_rota,  yF_rota             
  LONG a_pos, b_pos, c_pos, d_pos, e_pos, f_pos
  LONG p_a, p_b, p_c, p_d, p_e, p_f
 
  ' Geometrische Variablen
  LONG hA, hB, hC, hD, hE, hF
  LONG xA, xB, xC, xD, xE, xF
  LONG yA, yB, yC, yD, yE, yF
  LONG lA, lB, lC, lD, lE, lF, l_max
  LONG ph_A, ph_B, ph_C, ph_D, ph_E, ph_F
  LONG RiA, RiB, RiC, RiD, RiE, RiF, Ri_min
  LONG i_A, i_B, i_C, i_D, i_E, i_F
  LONG Ri_max
  LONG wQ, wL, hBasis, wx, wy
 
  BYTE A_back, B_back, C_back, D_back, E_back, F_back
  BYTE c_a, c_b, c_c, c_d, c_e, c_f
                                                     
pub main
  init
  speed := 3000
'-------------------------------------------------------------------------------------------------------------------------
'### INITIALPOSITION ANFAHREN
  coginit(6,@moveServo,@new_peri_r)
  coginit(7,@moveServo,@new_peri_l)                      ' Anfahren der Initalposition
  coginit(5,setpulse,@stack_setpulse)
 
  repeat
    busy_led                                            ' Bestätigen, dass bereit
    if ina[6] == 1
      waitcnt(2_000_000  + cnt)
      outa[26]~
      outa[27]~~
      quit
'-------------------------------------------------------------------------------------------------------------------------

  hBasis := 193.0
  wQ := 0.0
  wL := 0.0
  wx := 0.0
  wy := 0.0
 
  xA := x_off
  yA := 0.0
  hA := 0.0
 
  xB := x_off
  yB := 0.0
  hB := 0.0
 
  xC := x_off
  yC := 0.0
  hC := 0.0
 
  xD := x_off
  yD := 0.0
  hD := 0.0
 
  xE := x_off
  yE := 0.0
  hE := 0.0
  xF := x_off
  yF := 0.0
  hF := 0.0
  IK
'------------------------------------------------------------------------------------------------------------------------- 
  repeat
    busy_led                                            ' Bestätigen, dass bereit
    if ina[6] == 1
      waitcnt(2_000_000  + cnt)
      outa[26] := OFF
      quit

  hBasis := 193.0
  wQ    := 0
  wL    := 0
  wx    := 0
  wy    := 0

  Richtung(0.0, 0.0, -350.0, 0.0, 45.0, 1.0) 'Richtung(y, x,w_y, v_x, teta, speed) 
  'Vorgabe

  repeat 
    outa[27] := OFF
    waitcnt(10_000_000  + cnt)
    outa[27] := ON
    waitcnt(10_000_000  + cnt)
    repeat
      outa[27] := ON

 

' ######################################################################################################
' #####################  EBENE 2 #######################################################################
' ######################################################################################################
PUB Richtung(y,x,w,v,teta,speed_2)|alpha_trans,distance,steps_trans,steps_rota,steps_all, step_counter,Xi,sign    ' v,w, 
 
  alpha_trans := math.Atan2(y,x)  'y,x in Rad
  if (y== 0.0 AND x < 0.0)
    alpha_trans :=math.Radians(180.0)

  if(x==0.0)AND(y==0.0)
    distance := 0.0
  else
    distance := math.FSqr(math.FAdd(math.pow(x, 2.0), math.Pow(y,2.0)))

  if ((alpha_trans => -1.0471 AND alpha_trans =< -2.0943) OR (alpha_trans => 1.0471 AND alpha_trans =< 2.093)) AND(teta==0.0) 
    Ri_Max := 150.0
  elseif (teta == 0.0)
    Ri_Max := 90.0 
  else
    Ri_Max := 90.0 
 
  lA := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))
  lB := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(w,                  2.0)))
  lC := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(math.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
  lD := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off,          110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
  lE := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off,          110.0), v),2.0), math.Pow(w,                  2.0)))
  lF := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x_off,          110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))

  l_max := math.FMax(lA, lB)   
  l_max := math.FMax(l_max, lC)   
  l_max := math.FMax(l_max, lD)   
  l_max := math.FMax(l_max, lE)   
  l_max := math.FMax(l_max, lF)     

  Xi := math.Degrees(math.ATan(math.FDiv(Ri_max,math.FMul(2.0,l_max))))
 
  if (teta == 0.0)             
    steps_rota := math.FRound(0.0)
    Ri_min := Ri_max
   
    i_A := math.FDiv(Ri_Max,14.0)
    i_B := i_A
    i_C := i_A
    i_D := i_A
    i_E := i_A
    i_F := i_A
   
  else
    steps_rota  := math.FRound(math.FAbs(math.FDiv(teta, Xi)))
    steps_rota++

    RiA := math.FMul(lA,math.FMul(math.tan(math.Radians(Xi)),2.0))
    RiB := math.FMul(lB,math.FMul(math.tan(math.Radians(Xi)),2.0))
    RiC := math.FMul(lC,math.FMul(math.tan(math.Radians(Xi)),2.0))
    RiD := math.FMul(lD,math.FMul(math.tan(math.Radians(Xi)),2.0))
    RiE := math.FMul(lE,math.FMul(math.tan(math.Radians(Xi)),2.0))
    RiF := math.FMul(lF,math.FMul(math.tan(math.Radians(Xi)),2.0)) 

    Ri_min :=math.FMin(RiA,RiB)
    Ri_min :=math.FMin(Ri_min,RiC)
    Ri_min :=math.FMin(Ri_min,RiD)
    Ri_min :=math.FMin(Ri_min,RiE)
    Ri_min :=math.FMin(Ri_min,RiF)

    i_A := math.FDiv(RiA,14.0)
    i_B := math.FDiv(RiB,14.0)
    i_C := math.FDiv(RiC,14.0)
    i_D := math.FDiv(RiD,14.0)
    i_E := math.FDiv(RiE,14.0)
    i_F := math.FDiv(RiF,14.0)
   

  if(teta > 0.0)
    sign :=  1.0
  else
    sign := -1.0

  if(distance == 0.0)
    steps_trans := math.FRound(0.0)
  else
    steps_trans := math.FRound(math.FDiv(distance, Ri_min))     
    steps_trans++

  ph_A := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FAdd(w,115.0),lA)))))   
  ph_B := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(w,lB)))))
  ph_C := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FSub(w,115.0),lC)))))
  ph_D :=          math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FSub(w,115.0),lD))))
  ph_E :=          math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(w,lE))))
  ph_F :=          math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(math.FAdd(w,115.0),lF))))

  case speed_2 
    1.0: 
        a_pos := -2.0
        b_pos := -4.0
        c_pos :=  8.0
        d_pos := -1.0
        e_pos :=  5.0
        f_pos := -7.0
    1.75:     
        a_pos :=  8.25
        b_pos :=  1.75
        c_pos := -5.25
        d_pos :=  1.75
        e_pos :=  8.75
        f_pos := -5.25
    2.5:     
        a_pos :=  10.5
        b_pos := -3.5
        c_pos :=  10.5
        d_pos := -3.5
        e_pos :=  10.5
        f_pos := -3.5
 
  if (steps_trans => steps_rota)
    steps_all := steps_trans
  else
    steps_all := steps_rota

  c_a := 0
  c_b := 0
  c_c := 0
  c_d := 0
  c_e := 0
  c_f := 0

  p_a := 0
  p_b := 0
  p_c := 0
  p_d := 0
  p_e := 0
  p_f := 0

  Step_counter := 0
 
  repeat until(step_counter == steps_all)
    if (step_counter == steps_rota)
      teta := 0.0
    if (step_counter == steps_trans)
      distance := 0.0
     
    if (a_pos > 7.0)
      A_back := 1
      case c_a
        0:
          p_a := 7.0
          hA := -50.0
          c_a++
        1:
          p_a := 0.0
          hA := -50.0
          c_a++
        2:
          p_a := -7.0
          hA := -50.0
          c_a++         
        3:
          p_a := -7.0
          hA := 0.0
          a_pos := -7.0
          c_a := 0
    else 
        A_back := 0
        p_a := a_pos
        hA := 0.0

    if (b_pos > 7.0)
      B_back := 1 
      case c_b
        0:
          p_b := 7.0
          hb := -50.0
          c_b++
        1:
          p_b := 0.0
          hb := -50.0
          c_b++
        2:
          p_B := -7.0
          hB := -50.0
          c_b++         
        3:
          p_b := -7.0
          hB := 0.0
          b_pos := -7.0
          c_b := 0
    else 
        B_back := 0 
        p_b := b_pos
        hB := 0.0

    if (c_pos > 7.0)
      C_back := 1 
      case c_c
        0:
          p_c := 7.0
          hC := -50.0
          c_c++
        1:
          p_c := 0.0
          hC := -50.0
          c_c++
        2:
          p_C := -7.0
          hC := -50.0
          c_c++         
        3:
          p_c := -7.0
          hC := 0.0
          C_pos := -7.0
          c_C := 0
    else
        C_back := 0   
        p_c := c_pos
        hC := 0.0

    if (d_pos > 7.0)
      D_back := 1 
      case c_d
        0:
          p_d := 7.0
          hD := -50.0
          c_d++
        1:
          p_d := 0.0
          hD := -50.0 '150
          c_d++
        2:
          p_D := -7.0
          hD := -50.0
          c_D++         
        3:
          p_d := -7.0
          hD := 0.0
          D_pos := -7.0
          c_D := 0
    else
        D_back := 0   
        p_d := d_pos
        hD := 0.0     

    if (e_pos > 7.0)
      E_back := 1 
      case c_e
        0:
          p_e := 7.0
          hE := -50.0
          c_e++
        1:
          p_e := 0.0
          hE := -50.0
          c_e++
        2:
          p_E := -7.0
          hE := -50.0
          c_e++         
        3:
          p_e := -7.0
          hE := 0.0
          e_pos := -7.0
          c_e := 0
    else
        E_back := 0   
        p_e := e_pos
        hE := 0.0

    if (f_pos > 7.0)
      F_back := 1 
      case c_f
        0:
          p_f := 7.0
          hF := -50.0
          c_f++
        1:
          p_f := 0.0
          hF := -50.0
          c_f++
        2:
          p_F := -7.0
          hF := -50.0
          c_F++         
        3:
          p_F := -7.0
          hF := 0.0
          F_pos := -7.0
          c_f := 0

    else
        F_back := 0   
        p_f := f_pos
        hF := 0.0
   
       
    if (distance == 0.0)
      xA := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A),math.cos(math.Radians(ph_A))),math.FMul(i_A,sign)))
      xB := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B),math.cos(math.Radians(ph_B))),math.FMul(i_B,sign)))
      xC := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C),math.cos(math.Radians(ph_C))),math.FMul(i_C,sign)))
      xD := math.FSub(x_off,math.FMul(math.FMul(p_D          ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign)))
      xE := math.FSub(x_off,math.FMul(math.FMul(p_E          ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign)))
      xF := math.FSub(x_off,math.FMul(math.FMul(p_F          ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign)))

      yA := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.Radians(ph_A)) ),math.FMul(i_A,sign))
      yB := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.Radians(ph_B)) ),math.FMul(i_B,sign))
      yC := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.Radians(ph_C)) ),math.FMul(i_C,sign))     
      yD := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.Radians(ph_D)) ),math.FMul(i_D,sign))
      yE := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.Radians(ph_E)) ),math.FMul(i_E,sign))
      yF := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.Radians(ph_F)) ),math.FMul(i_F,sign))

     
    else
      if (teta == 0.0)

        xA := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul(i_A,math.cos(alpha_trans))))
        xB := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul(i_B,math.cos(alpha_trans))))
        xC := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul(i_C,math.cos(alpha_trans))))
        xD := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.cos(alpha_trans))))
        xE := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.cos(alpha_trans))))
        xF := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.cos(alpha_trans))))

        yA := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(alpha_trans)))
        yB := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(alpha_trans)))           
        yC := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(alpha_trans)))
        yD := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(alpha_trans)))
        yE := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(alpha_trans))) 
        yF := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(alpha_trans)))
 
      else
        xA_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A),math.cos(math.Radians(ph_A))),math.FMul(i_A,sign)))
        xB_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B),math.cos(math.Radians(ph_B))),math.FMul(i_B,sign)))
        xC_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C),math.cos(math.Radians(ph_C))),math.FMul(i_C,sign)))
        xD_rota := math.FSub(x_off,math.FMul(math.FMul(p_D          ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign)))
        xE_rota := math.FSub(x_off,math.FMul(math.FMul(p_E          ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign)))
        xF_rota := math.FSub(x_off,math.FMul(math.FMul(p_F          ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign))) 
        yA_rota := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.Radians(ph_A)) ),math.FMul(i_A,sign))
        yB_rota := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.Radians(ph_B)) ),math.FMul(i_B,sign))
        yC_rota := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.Radians(ph_C)) ),math.FMul(i_C,sign))     
        yD_rota := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.Radians(ph_D)) ),math.FMul(i_D,sign))
        yE_rota := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.Radians(ph_E)) ),math.FMul(i_E,sign))
        yF_rota := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.Radians(ph_F)) ),math.FMul(i_F,sign))

        xA_trans := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul(i_A,math.cos(alpha_trans))))
        xB_trans := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul(i_B,math.cos(alpha_trans))))
        xC_trans := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul(i_C,math.cos(alpha_trans))))
        xD_trans := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.cos(alpha_trans))))
        xE_trans := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.cos(alpha_trans))))
        xF_trans := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.cos(alpha_trans))))
        yA_trans := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(alpha_trans)))
        yB_trans := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(alpha_trans)))           
        yC_trans := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(alpha_trans)))
        yD_trans := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(alpha_trans)))
        yE_trans := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(alpha_trans))) 
        yF_trans := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(alpha_trans)))

        xC := math.FAdd(math.FDiv(math.FSub(xC_trans, xC_rota),2.0),xC_rota)
        yC := math.FAdd(math.FDiv(math.FSub(yC_trans, yC_rota),2.0),yC_rota)
        xD := math.FAdd(math.FDiv(math.FSub(xD_trans, xD_rota),2.0),xD_rota)
        yD := math.FAdd(math.FDiv(math.FSub(yD_trans, yD_rota),2.0),yD_rota)    }}


    IK
    a_pos := math.FAdd(a_pos, speed_2)
    b_pos := math.FAdd(b_pos, speed_2)
    c_pos := math.FAdd(c_pos, speed_2)
    d_pos := math.FAdd(d_pos, speed_2)
    e_pos := math.FAdd(e_pos, speed_2)
    f_pos := math.FAdd(f_pos, speed_2)

   
' ######################################################################################################
' #####################  EBENE 1c #######################################################################
' ######################################################################################################

pub move|i
    i:=0
    repeat 3
      if Aziel[i] > Alast[i]                                    ' Servo-bewegung vorwärts
        Asign[i] := +1
      if Aziel[i] < Alast[i]                                    ' Servo-bewegung rückwärts
        Asign[i] := -1
       
      if Bziel[i] > Blast[i]                                    ' Servo-bewegung vorwärts
        Bsign[i] := +1
      if Bziel[i] < Blast[i]                                    ' Servo-bewegung rückwärts
        Bsign[i] := -1
     
      if Cziel[i] > Clast[i]                                    ' Servo-bewegung vorwärts
        Csign[i] := +1
      if Cziel[i] < Clast[i]                                    ' Servo-bewegung rückwärts
        Csign[i] := -1
     
      if Dziel[i] > Dlast[i]                                    ' Servo-bewegung vorwärts
        Dsign[i] := +1
      if Dziel[i] < Dlast[i]                                    ' Servo-bewegung rückwärts
        Dsign[i] := -1
   
      if Eziel[i] > Elast[i]                                    ' Servo-bewegung vorwärts
        Esign[i] := +1
      if Eziel[i] < Elast[i]                                    ' Servo-bewegung rückwärts
        Esign[i] := -1 
   
      if Fziel[i] > Flast[i]                                    ' Servo-bewegung vorwärts
        Fsign[i] := +1
      if Fziel[i] < Flast[i]                                    ' Servo-bewegung rückwärts
        Fsign[i] := -1

      if A_back == 1
        ASign[i] := ASign[i]*5
      if B_back == 1
        BSign[i] := BSign[i]*5
      if C_back == 1
        CSign[i] := CSign[i]*5
      if D_back == 1
        DSign[i] := DSign[i]*5
      if E_back == 1
        ESign[i] := ESign[i]*5
      if F_back == 1
        FSign[i] := FSign[i]*5 
      i++

    repeat                                                    ' Bewegung 
      i:= 0                                                                       
      repeat 3
        Apos[i] += Asign[i]                                    ' Positionsänderung
        if ((Asign[i]>0.0) AND (Apos[i] > Aziel[i])) OR  ((Asign[i]<0.0) AND (Apos[i] < Aziel[i])) ' Ziel erreicht
          Asign[i]:=0 
        Bpos[i] += Bsign[i]                                    ' Positionsänderung
        if ((Bsign[i]>0.0) AND (Bpos[i] > Bziel[i])) OR  ((Bsign[i]<0.0) AND (Bpos[i] < Bziel[i])) ' Ziel erreicht
          Bsign[i]:=0
        Cpos[i] += Csign[i]                                    ' Positionsänderung
        if ((Csign[i]>0.0) AND (Cpos[i] > Cziel[i])) OR  ((Csign[i]<0.0) AND (Cpos[i] < Cziel[i])) ' Ziel erreicht
          Csign[i]:=0     
        Dpos[i] += Dsign[i]                              '
        if ((Dsign[i]>0.0) AND (Dpos[i] > Dziel[i])) OR  ((Dsign[i]<0.0) AND (Dpos[i] < Dziel[i])) ' Ziel erreicht 
          Dsign[i]:=0
        Epos[i] += Esign[i]                                    ' Positionsänderung
        if ((Esign[i]>0.0) AND (Epos[i] > Eziel[i])) OR  ((Esign[i]<0.0) AND (Epos[i] < Eziel[i])) ' Ziel erreicht
          Esign[i]:=0
        Fpos[i] += Fsign[i]                                    ' Positionsänderung
        if ((Fsign[i]>0.0) AND (Fpos[i] > Fziel[i])) OR  ((Fsign[i]<0.0) AND (Fpos[i] < Fziel[i])) ' Ziel erreicht
          Fsign[i]:=0
        i++
       
      setpulse                                                ' Servo einen Schritt bewegen
      repeat speed                                            ' Warten
     
    until ((Asign[0] == 0) AND (Asign[1] == 0) AND (Asign[2] == 0)AND(Bsign[0] == 0) AND (Bsign[1] == 0) AND (Bsign[2] == 0)AND (Csign[0] == 0) AND (Csign[1] == 0) AND (Csign[2] == 0)AND(Dsign[0] == 0) AND (Dsign[1] == 0) AND (Dsign[2] == 0)AND(Esign[0] == 0) AND (Esign[1] == 0) AND (Esign[2] == 0)AND(Fsign[0] == 0) AND (Fsign[1] == 0) AND (Fsign[2] == 0))                                  ' Bis zum Ziel und Schluss                             

' ######################################################################################################
' #####################  EBENE 1a/1b ###################################################################
' ######################################################################################################
   
PUB IK|i, ca, ca_2, alpha, alpha_PP, alpha_P, gamma, phi, dhAF, dhBE, dhCD, dhABC, dhDEF, dQ, dL, rA, rB, rC, rD, rE, rF, hhA, hhB, hhC, hhD, hhE, hhF

  '~~~~~ BERECHNUNG KIPPEN UND NEIGEN ~~~~~
  dQ := math.Sin(math.Radians(wQ))
  dL := math.Sin(math.Radians(wL))
  'Prüfen ob -wy bzw. -wx korrekt ist)
  dhCD  :=  math.FMul(dQ, math.FSub(-230.0, -wy))
  dhBE  :=  math.FMul(dQ, math.FSub(0.0, -wy))
  dhAF  :=  math.FMul(dQ, math.FSub(+230.0, -wy)) 
  dhABC :=  math.FMul(dL, math.FSub(-110.0, -wx))
  dhDEF :=  math.FMul(dL, math.FSub(+110.0, -wx)) 

  hhA := math.FAdd(hBasis, dhAF)
  hhA := math.FAdd(hhA, dhABC)
  hhA := math.FAdd(hhA, hA)
  hhB := math.FAdd(hBasis, dhBE)
  hhB := math.FAdd(hhB, dhABC)
  hhB := math.FAdd(hhB, hB)
  hhC := math.FAdd(hBasis, dhCD)
  hhC := math.FAdd(hhC, dhABC)
  hhC := math.FAdd(hhC, hC)
  hhD := math.FAdd(hBasis, dhCD)
  hhD := math.FAdd(hhD, dhDEF)
  hhD := math.FAdd(hhD, hD)
  hhE := math.FAdd(hBasis, dhBE)
  hhE := math.FAdd(hhE, dhDEF)
  hhE := math.FAdd(hhE, hE)
  hhF := math.FAdd(hBasis, dhAF)
  hhF := math.FAdd(hhF, dhDEF)
  hhF := math.FAdd(hhF, hF)
 
  rA := math.FSqr(math.FAdd(math.FMul(xA, xA), math.FMul(yA,yA)))
  rB := math.FSqr(math.FAdd(math.FMul(xB, xB), math.FMul(yB,yB)))
  rC := math.FSqr(math.FAdd(math.FMul(xC, xC), math.FMul(yC,yC)))
  rD := math.FSqr(math.FAdd(math.FMul(xD, xD), math.FMul(yD,yD)))
  rE := math.FSqr(math.FAdd(math.FMul(xE, xE), math.FMul(yE,yE)))
  rF := math.FSqr(math.FAdd(math.FMul(xF, xF), math.FMul(yF,yF)))

  '~~~~~ BERECHNUNG SERVOWINKEL ALPHA GAMMA PHI
  '----- Länge von Bein A ----
  ca_2 := math.FAdd(math.FMul(hhA, hhA), math.FMul(rA,rA))
  ca  := math.FSqr(ca_2)
  '----- Winkel von Hüfte A0 ---- 
  phi := math.Degrees(math.Atan2(xA, yA))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Aziel[0] := math.FRound(phi)
  Aziel[0] := Aziel[0] +0
  '----- Winkel Schulter A1 ----
  alpha_P  := math.ATan(math.FDiv(rA, hhA))
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Aziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Aziel[1] := Aziel[1] - 60
  '----- Winkel Fuss A2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Aziel[2] := math.FSub(2910, math.FRound(gamma))
  Aziel[2] := Aziel[2] +80

  '---_- Länge von Bein B ----
  ca_2 := math.FAdd(math.FMul(hhB, hhB), math.FMul(rB,rB))
  ca  := math.FSqr(ca_2)
  '---_- Winkel von Hüfte B0 ---- 
  phi := math.Degrees(math.Atan2(xB, yB))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Bziel[0] := math.FRound(phi)
  Bziel[0] := Bziel[0] +40
  '----- Winkel Schulter B1 ----
  alpha_P  := math.ATan(math.FDiv(rB, hhB)  )
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Bziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Bziel[1] := Bziel[1] -40
  '----- Winkel Fuss B2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Bziel[2] := math.FSub(2910, math.FRound(gamma))
  Bziel[2] := Bziel[2] +100 
 
  '----- Länge von Bein C ----
  ca_2 := math.FAdd(math.FMul(hhC, hhC), math.FMul(rC,rC))
  ca  := math.FSqr(ca_2)         
  '----- Winkel von Hüfte C0 ---- 
  phi := math.Degrees(math.Atan2(xC, yC))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Cziel[0] := math.FRound(phi)
  Cziel[0] := Cziel[0] +60
  '----- Winkel Schulter C1 ----
  alpha_P  := math.ATan(math.FDiv(rC, hhC)  )
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Cziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Cziel[1] := Cziel[1] +80
  '----- Winkel Fuss C2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Cziel[2] := math.FSub(2910, math.FRound(gamma))
  Cziel[2] := Cziel[2] +100

  '----- Länge von Bein D ----
  ca_2 := math.FAdd(math.FMul(hhD, hhD), math.FMul(rD,rD))
  ca  := math.FSqr(ca_2)
  '----- Winkel von Hüfte D0 ---- 
  phi := math.Degrees(math.Atan2(xD, yD))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Dziel[0] := math.FSub(2910, math.FRound(phi)) 
  Dziel[0] := Dziel[0] +80
  '----- Winkel Schulter D1 ----
  alpha_P  := math.ATan(math.FDiv(rD, hhD))
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Dziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Dziel[1] := math.FSub(2910, Dziel[1])
  Dziel[1] := Dziel[1]  +5 
  '----- Winkel Fuss D2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Dziel[2] := math.FRound(gamma)
  Dziel[2] := Dziel[2]  -80 

  '----- Länge von Bein E ----
  ca_2 := math.FAdd(math.FMul(hhE, hhE), math.FMul(rE,rE))
  ca  := math.FSqr(ca_2)
  '----- Winkel von Hüfte E0 ---- 
  phi := math.Degrees(math.Atan2(xE, yE))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Eziel[0] := math.FSub(2910, math.FRound(phi)) 
  Eziel[0] := Eziel[0] +70
  '----- Winkel Schulter E1 ----
  alpha_P  := math.ATan(math.FDiv(rE, hhE))
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Eziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Eziel[1] := math.FSub(2910, Eziel[1]) 
  Eziel[1] := Eziel[1]  -20
  '----- Winkel Fuss E2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Eziel[2] := math.FRound(gamma)
  Eziel[2] := Eziel[2]  +0

  '----- Länge von Bein F ----
  ca_2 := math.FAdd(math.FMul(hhF, hhF), math.FMul(rF,rF))
  ca  := math.FSqr(ca_2)
  '----- Winkel von Hüfte F0 ---- 
  phi := math.Degrees(math.Atan2(xF, yF))
  phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
  Fziel[0] := math.FSub(2910, math.FRound(phi)) 
  Fziel[0] := Fziel[0] +50
  '----- Winkel Schulter F1 ----
  alpha_P  := math.ATan(math.FDiv(rF, hhF))
  alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
  alpha    := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
  Fziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0)) 
  Fziel[1] := math.FSub(2910, Fziel[1]) 
  Fziel[1] := Fziel[1] -100 
 '----- Winkel Fuss F2 ----
  gamma    := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
  gamma    := math.Degrees(gamma)
  gamma    := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
  Fziel[2] := math.FRound(gamma)
  Fziel[2] := Fziel[2] -0
                 
  move

  i:=0
  repeat 3
    Alast[i] := Aziel[i]
    Blast[i] := Bziel[i]
    Clast[i] := Cziel[i]
    Dlast[i] := Dziel[i]
    Elast[i] := Eziel[i]
    Flast[i] := Fziel[i]
    i++
 
pub busy_led                                           
  !outa[26]                                              ' Ausgang invertieren
  waitcnt(3_000_000 + cnt)                              ' 300ms warten
 
pub ready_led
  outa[26]~~                                            ' LED ON
  waitcnt(100_000_000 + cnt)
  outa[26]~


pub init|i
  math.start

  '------------------Feste-Parameter----------------------------------------------------------------------------------------
  periode := ((clkfreq/1000000)*20000)                  'Länge der gesamten Periode 
  PinStart_r:=|<16
  PinStart_l:=|<7
  dira[7..15]~~
  pin_l:= dira
  dira[16..24]~~
  pin_r:= dira   

  dira[27]~~                                            'Ausgang für LED - Gelb
  dira[26]~~                                            'Ausgang für LED - Grün
  dira[25]~                                            'Eingang Button
  dira[6]~
 
  'Initialposition
  Aini[0] := 1490
  Aini[1] := 1490
  Aini[2] := 1490
  Bini[0] := 1490
  Bini[1] := 1490
  Bini[2] := 1490
  Cini[0] := 1490
  Cini[1] := 1490
  Cini[2] := 1490
  Dini[0] := 1490
  Dini[1] := 1490
  Dini[2] := 1490
  Eini[0] := 1490
  Eini[1] := 1490
  Eini[2] := 1490
  Fini[0] := 1490
  Fini[1] := 1490
  Fini[2] := 1490


  'Min, Max - Werte für Servobewegung und Positionen anfahren.
  i:=0
  repeat 3
    Apos[i] #>= 809                                          ' Untere Servogrenze
    Apos[i] <#= 2169   
    Alast[i] := Aini[i]
    Apos[i]  := Aini[i]

    Bpos[i] #>= 809                                          ' Untere Servogrenze
    Bpos[i] <#= 2169   
    Blast[i] := Bini[i]
    Bpos[i]  := Bini[i]

    Cpos[i] #>= 809                                          ' Untere Servogrenze
    Cpos[i] <#= 2169   
    Clast[i] := Cini[i]
    Cpos[i]  := Cini[i]

    Dpos[i] #>= 809                                          ' Untere Servogrenze
    Dpos[i] <#= 2169   
    Dlast[i] := Dini[i]
    Dpos[i]  := Dini[i]

    Epos[i] #>= 809                                          ' Untere Servogrenze
    Epos[i] <#= 2169   
    Elast[i] := Eini[i]
    Epos[i]  := Eini[i]
 
    Fpos[i] #>= 809                                          ' Untere Servogrenze
    Fpos[i] <#= 2169   
    Flast[i] := Fini[i]
    Fpos[i]  := Fini[i]
    i++
  setpulse

   
' ######################################################################################################
' #####################  EBENE 0 #######################################################################
' ######################################################################################################

pub setpulse|i,j 'Bewegen der Servos, berechnen Impulslänge 
  new_peri_r:= periode
  new_peri_l:= periode

  '-----------------LINKE SEITE-----------------------------------------------------
  i:=0
  j:=0
  repeat  3
    high_pulse_l[i] := ((clkfreq/ 1000000)  * Fpos[j])          'aktuelle pulselänge
    new_peri_l-=high_pulse_l[i]
    i+=1
    j+=1
  j:=0 
  repeat  3
    high_pulse_l[i] := ((clkfreq/ 1000000)  * Epos[j])          'aktuelle pulselänge
    new_peri_l-=high_pulse_l[i]
    i+=1
    j+=1
  j:=0
  repeat  3
    high_pulse_l[i] := ((clkfreq/ 1000000)  * Dpos[j])          'aktuelle pulselänge
    new_peri_l-=high_pulse_l[i]
    i+=1
    j+=1
 
  '-----------------RECHTE SEITE-----------------------------------------------------
  i:=0
  j:=0
  repeat  3
    high_pulse_r[i] := ((clkfreq/ 1000000)  * Cpos[j])          'aktuelle pulselänge
    new_peri_r-=high_pulse_r[i]
    i+=1
    j+=1
  j:=0
  repeat  3
    high_pulse_r[i] := ((clkfreq/ 1000000)  * Bpos[j])          'aktuelle pulselänge
    new_peri_r-=high_pulse_r[i]
    i+=1
    j+=1
  j:=0
  repeat  3
    high_pulse_r[i] := ((clkfreq/ 1000000)  * Apos[j])          'aktuelle pulselänge
    new_peri_r-=high_pulse_r[i]
    i+=1
    j+=1 
 
DAT  'Ausgeben der Pulse

 ORG      0
moveServo    mov      Index,                  par                    'die adresse des cograms wird nach index geschreiben

---

Gestern habe ich festgestellt, dass man die Programmierschnittstelle (RS232) des Propeller Chips, nach dem flashen auch weiter verwenden kann und darüber Daten zurück an den PC schicken (die entsprechenden Bibliotheken sind schon fertig). Die Entwicklungsumgebung hat eine entsprechende Empfangskonsole bereits implementiert und damit ist der Austausch zwischen Controller und PC wirklich leicht zu realisieren.
Im Moment lass ich mir die Berechneten Werte für die einzelnen Beine zurück geben und kann damit quasi die Telemetrie des Vinculums überwachen und auch ob alles korrekt berechnet wurde. Feine Sache aber: Ich brauch dazu im Moment ein Kabel :(

Frage an die Runde: Welche einfachen und billigen Möglichkeiten gibt es um das RS232 Kabel durch Funk zu ersetzen? Die notwendige Reichweite beträgt vielleicht 10m, nur indoor und im gleichen Raum. Bidirektional wäre fein, dann könne ich auch über Funk flashen.

Ich habe jetzt das ZigBee Zeugs mal angeschaut, bin mir aber nicht sicher ob ich damit meine Datenübertragung realisieren kann. Hat da jemand mehr Erfahrung damit? Tutorials?

Schau dir mal die NRF24L01 an, haben zwar eine SPI Schnittstelle, aber die hat der Propeller ja auch.

Vorteil,
- sie sind günstiger als Zigbee
- Automatische checksumme erzeugen und kümmern sich auch bei fehlern um das erneute senden.
- Reichweiten von wenigen Metern bis 1km möglich.
- Baudrate auf kurze distanz mit bis zu 2Mbit

Wenn Du lust auf basteln hast hätte ich hier eine Unidirektionale Sache hier liegen. Von HM Funktechnik die 70 RX-M und die 70 TX-M.

Nachtrag: Hier gerade gefunden ! http://plischka.at/Funkmodul_fuer_hohe_Reichweiten.html

@Slowly meinst du zum ausprobieren? Ich habe bald wieder arbeit für dich, denn ich will dem Hexa noch einen Sensorkopf spendieren, dazu braucht es aber ein Aluminium Gestell drum herum (1,5mm Stärke).

Ich habe schon gesehen, dass mein Propeller Chip auch ZigBee unterstützt und diese Funkübertragung von Parallax für den Chip angeboten wird. Die Lösung wäre etwas billiger, da ich eigentlich keine 200€ für eine Spielerei ausgeben will. Es gibt keine Notwendigkeit das Signal über Funk zu schicken, es wäre nur bequem und high-end :)