Question Sainsmart self-leveling Robot code.

[Notebook]

I'm doing a blog on this kit, mostly to get a handle on PID.
https://en.wikipedia.org/wiki/PID_controller.

I thought I would put code questions on the forum for a quicker response.
This is the code for the hand-controller.

#include "SPI.h"
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "Wire.h" 
 
long Joystick_1_X;
long Joystick_1_Y;
long Joystick_2_X;
long Joystick_2_Y;
long Joystick_1;
long Joystick_2;

void setup(){
  Serial.begin(115200);
  Mirf.spi = &MirfHardwareSpi;  
  Mirf.init(); 
  Mirf.setTADDR((byte *)"serv1");
  Mirf.payload = sizeof(long);
  Mirf.config(); 
 }

void loop(){
  Joystick_1_X = analogRead(A0);
  Joystick_1_Y = analogRead(A1);
  //Joystick_2_X = analogRead(A2);
  //Joystick_2_Y = analogRead(A3);
  
  //Serial.print("Joystick_1_X=");Serial.print(Joystick_1_X); 
  Serial.print("  Joystick_1_Y=");Serial.print(Joystick_1_Y);
  Serial.print("  Joystick_2_X=");Serial.print(Joystick_2_X); 
  Serial.print("  Joystick_2_Y=");Serial.print(Joystick_2_Y);
  
  Joystick_1_X = Joystick_1_X << 16;
  Joystick_1 = Joystick_1_X | Joystick_1_Y;
 
  //Serial.print("  Joystick_1_X=");Serial.print(Joystick_1_X);
  Serial.print("  Joystick_1=");Serial.print(Joystick_1);
  
  //Joystick_2_X = Joystick_2_X << 16;
  //Joystick_2 = - (Joystick_2_X | Joystick_2_Y);
  
  //Serial.print("  Joystick_2_X=");Serial.print(Joystick_2_X);
  //Serial.print("  Joystick_2=");Serial.println(Joystick_2);
  
  Mirf.send((byte *)&Joystick_1);
  //delay(10);
  //Mirf.send((byte *)&Joystick_2);
  while(Mirf.isSending()){
  }
}

Its only sending one byte for the two axis of the joystick? I'm guessing its doing something clever with the left shifts and "OR"ing the two axis?

Could someone talk me throght this please.

thanks, N.

 

[kamaz]

There is some serious WTF here.

This is what his code is doing (result of each step in bold)

Joystick_1_X = analogRead(A0);
Joystick_1_X = 00000000 00000000 000000XX XXXXXXXX

Joystick_1_Y = analogRead(A1);
Joystick_1_Y = 00000000 00000000 000000YY YYYYYYYY

Joystick_1_X = Joystick_1_X << 16;
Joystick_1_X = 000000XX XXXXXXXX 00000000 00000000

Joystick_1 = Joystick_1_X | Joystick_1_Y;
Joystick_1 = 000000XX XXXXXXXX 000000YY YYYYYYYY

That part is good. The WTF comes next:

Mirf.send((byte *)&Joystick_1);

this is equivalent to:

long *pLong = &Joystick_1;
byte *pByte = (byte*) pLong;
Mirf.send( pByte )

Now it all depends what Mirf.send(byte *) does.

If Mirf.send() transmits a single byte pointed by the argument then this will transmit the youngest byte of Joystick_1, i.e. YYYYYYYY, i.e. the youngest 8 bits of the Y position (AVR is little-endian). Yes, this makes no sense. If he wanted to transmit the entire long byte-by-byte he should rather do

Mirf.send( pByte )
Mirf.send( pByte + 1 )
Mirf.send( pByte + 2)
Mirf.send( pByte + 3)

A secondary WTF is why Mirf.send() takes byte* instead of byte. This is taking stuff out of your left pocket using your right hand. There is no sense to go through the pointer, because a single byte can be obviously passed through stack during a function call. In fact since on AVR pointers are 16-bit, then he is shuffling two bytes through the stack and a pointer dereference instead of passing a single byte directly. Performance discussions aside, this clearly demonstrates these people do not know what they are doing.

On the other hand, if Mirf.send() expects a pointer to a NULL-terminated by string (think char*, in fact byte = unsigned char) then Mirf.send() this will walk through the stack and send everything starting at pByte and going upwards until it hits a zero byte... somewhere.

 

[Notebook]

Thanks for your help kamaz, this obviously out of my league. I've done a bit of C++ but nothing at bit-shifting or using a PIC. Its quite fun though, the Arudino IDE is straightforward, and I can modify and download the code to the PIC reliably.

This is the receiving end from the robot code:

oid Recive(){
  if(!Mirf.isSending() && Mirf.dataReady()){
    Mirf.getData((byte *)&data);
    Mirf.rxFifoEmpty();
    
    x = data >> 16;
    y = data & 0x0000ffff;
    
    if(x >= 520){
      Run_Speed_K = map(x, 520, 1023, 0, 100);
      Run_Speed_K = Run_Speed_K / 50;
      Run_Speed = Run_Speed + Run_Speed_K;
    }
    else if(x <= 480){
      Run_Speed_K = map(x, 480, 0, 0, -100);
      Run_Speed_K = Run_Speed_K / 50;
      Run_Speed = Run_Speed + Run_Speed_K;
    }
    else{
      Run_Speed_K = 0;
    }
    if(y >= 520){
      Turn_Speed = map(y, 520, 1023, 0, 20);
    }
    else if(y <= 480){
      Turn_Speed = map(y,480,0,0,-20);
    }
    else{
      Turn_Speed = 0;
    }
  }
  else{
    x = y = 500;
  }
}

I see x and y get made from right-shifting the received value, and "ANDing" the bottom 4 bits?
The robot doesn't have proportional steering, its just left/right, so I'm guessing its using the top 4 bits as a left/right value in the data byte?

EDIT, just seen that it is a 32 bit value, and from your description, its stuffing the x value into the top 16 bits. its the" Mirf.send((byte *)&Joystick_1);" that threw me.
I'm not sure how sending a byte gets you a32 bit value?


N.

 

[jedidia]

I'm not sure how sending a byte gets you a32 bit value?

Well, I would assume the reason why Mirf.send is taking a byte* as argument is that it expects a byte array of certain length to be behind it... a bit risky, but not out of the ordinary at such a low level. I can only guess that the reason for passing the data as a byte array instead of as an integer or a long is to achieve compiler independence (as these types are not the same length through all compilers).

 

[Notebook]

Thanks jedidia, from what I've read on the web, there are a lot of unhappy people who bought this kit. Besides the absence of any instructions, there are lots of comments about no support from Sainsmart.
I'm not too bothered, it'll do what what I want from it, a test bed for PID control.
Its starting already! Bit shifting and pointers to arrays, way out my depth, but its good fun...where's my lead-free solder aka Tin.

N.

 

[kamaz]

Not 4 bits but 4 nibbles (hex digits) i.e. 16 bits.

Assume

data = 000000XX XXXXXXXX 000000YY YYYYYYYY

x = data >> 16;
yields

x = 00000000 00000000 000000XX XXXXXXXX
(shift right by 16 padding with zeros from the left)

as for
y = data & 0x0000ffff;

0x0000ffff is hex for 00000000 00000000 11111111 11111111

thus

  000000XX XXXXXXXX 000000YY YYYYYYYY
& 00000000 00000000 11111111 11111111
= 00000000 00000000 000000YY YYYYYYYY

I'm not sure how sending a byte gets you a32 bit value?

It doesn't obviously.

If this code works, then it means Mirf.send() transmits four bytes starting from the supplied pointer. Maybe there is some other call which tells send() how many bytes it shoud transmit once it's called?

I have trouble finding docs for the Mirf.* calls. Do you have any?

 

[Notebook]

http://forum.arduino.cc/index.php?topic=59636.0

Found this on the Ardino forum

Strange, did something wrong and deleted most of my previous message, and can't remember what it was!

I don't want anyone to waste time on this, kamaz pointed out that what I thought was just a byte is pointing to a 32 bit value, so that explains how the x and y values are getting across the rf link.

Allegedly, the robot can stand by itself without the controller being powered, so that's the next step. I;ve powered it up and it is driving one motor back and forth as it passes through the vertical. The other motor stops, but goes in the same direction. Not sure if this is correct.
I've swapped the motor drive signal, and the fault? transfers, so nothing wrong with the hardware.


N.

 

[kamaz]

Thanks. Figured it out. The number of bytes to send/receive is controlled by the payload variable in the Mirf object.

This is why in setup() (see post #1) he does

Mirf.payload = sizeof(long);

to change the default length of 16. So his code is correct after all.

That said, if I was doing this, I would go along these lines:

// Common for both transmitter and receiver 
typedef struct _packet {
  int posX;
  int posY;
} packet_t;

packet_t packet;

setup() { 
  /.../
  Mirf.payload = sizeof(packet_t);
  /.../
}

// Transmitter:
loop() {
  /.../
  packet.posX = analogRead(A0);
  packet.posY = analogRead(A1);
  Mirf.send((byte*) &packet);
  /.../
}

// Receiver:
loop() {
  /.../
  Mirf.getData((byte*) &packet);
  Serial.print("posX = ");
  Serial.print(packet.posX);
  Serial.print("posY = ");
  Serial.print(packet.posY);
  Serial.println();
  /.../
}

 

[Notebook]

Thanks kamaz, I shall have a go with that code

N.

 

[Notebook]

Got some power on the robot, and its sort of balancing. Looks like it will take a lot of tweaking, but that's ok.
Reading in forums, one person thought the x/y axis were reversed, so I tried mine. Sure enough the x axis is forward/back, and y is left/right.

Looking at the robot code in #3 above, are x and y variables transposed?
Thanks, N.

EDIT:

took the plunge and swapped them over, seems to work in the right sense now.
Anymore stuff will be in the blog, be back here when I need help with the PID routine...