Tech Thursday

We’re picking up where we left off with last week’s Tech Thursday, A Computer Cart on a Mecanum Chassis – Part 1. This week, we will simply be posting the code for the Mecanum Computer Robot and its remote.

Here’s the code for the remote:

//  Name                : DeskMec_remote
//  Author		: SuperDroid Robots
//  Notice		: Copyright (c) 2016 SuperDroid Robots
//			: All Rights Reserved
//  Arduino		: Arduino Uno R3	
//  A Mirf example to use the nRF24L01 and Arduino to act as a wireless remote.
//  The Remote reads two analogs and transmits them

//  Hardware SPI:
//    MISO -> 12
//    MOSI -> 11
//    SCK -> 13
// 
//  Configurable:
//    CE -> 8
//    CSN -> 7

#include 
#include 
#include 
#include 

unsigned long timeSinceLastRx = 0;
bool tankMode = false, buttonPressed = false;

//Generate a packet.  Start character, 2 analogs and a checksum
unsigned char generatePacket () 
{
  unsigned char pre = 0;
  unsigned char analog0 = 0;
  unsigned char analog1 = 0;
  unsigned char analog2 = 0;
  unsigned char digital6 = 0;
  unsigned char packetChecksum;
  unsigned char packet[] = {0};
  
  packet[0] = pre = 83; // ASCII "S" start flag
  
  packet[1] = analog0 = analogRead(A0) >> 2;
  packet[2] = analog1 = analogRead(A1) >> 2;  //we only need 8 bit resolution
  packet[3] = analog2 = analogRead(A2) >> 2;
  packet[4] = digital6 = digitalRead(6);
  packet[5] = packetChecksum = analog1 + analog2 + analog0;
  
  return packet[0,1,2,3,4,5];
}

void setup()
{
  Serial.begin(9600);

  //Set the SPI Driver.
  Mirf.spi = &MirfHardwareSpi;
  
  //Setup pins / SPI.
  Mirf.init();
  
  //Configure reciving address.
  Mirf.setRADDR((byte *)"clie1");

  //Set payload on client and server must be the same.
  Mirf.payload = 6;

  //Write channel and payload config then power up reciver
  Mirf.config();

  //Serial.println("Beginning ... "); 
}

void loop()
{
  unsigned long time = millis();
  unsigned char txPacket[6];
  unsigned long rxPacket = 0;
  
  unsigned char pre = 0;
  unsigned char analog0 = 0;
  unsigned char analog1 = 0;
  unsigned char analog2 = 0;
  unsigned char digital6 = 0;
  unsigned char packetChecksum;
  unsigned char packet[6] = {0};
  
  bool btn = digitalRead(6);
  if(btn == 0 && !buttonPressed) {
    tankMode = !tankMode;
    buttonPressed = true;
  }
  else if(btn == 1) { buttonPressed = false; }
  
  if(tankMode) {  //tank mode
    packet[1] = analog0 = analogRead(A0) >> 2;
//    packet[2] = analog1 = analogRead(A2) >> 2;  //we only need 8 bit resolution
    packet[3] = analog2 = analogRead(A1) >> 2;
    packet[2] = 127;
  }
  else {  //vector mode
    packet[1] = analog0 = analogRead(A0) >> 2;
    packet[2] = analog1 = analogRead(A1) >> 2;  //we only need 8 bit resolution
    packet[3] = analog2 = analogRead(A2) >> 2;
  }
  
  packet[0] = pre = 83; // ASCII "S" start flag
  packet[4] = digital6 = digitalRead(6);
  packet[5] = packetChecksum = analog1 + analog2 + analog0;
  
  Mirf.setTADDR((byte *)"serv1");
//  Mirf.send((byte *)&txPacket);
  Mirf.send((byte *)packet);
  
  Serial.print(packet[0]); Serial.print(" ");
  Serial.print(packet[1]); Serial.print(" ");
  Serial.print(packet[2]); Serial.print(" ");
  Serial.print(packet[3]); Serial.print(" ");
  Serial.print(packet[4]); Serial.print(" ");
  Serial.println(packet[5]);
  
//  Serial.print("txPacket: ");
//  Serial.println(txPacket);

  while(Mirf.isSending()){ /*wait*/ }

  delay(100);
} 

And here’s the code for the robot:

//  Name		: DeskMec_robot.ino
//  Author		: SuperDroid Robots
//  Notice		: Copyright (c) 2016 SuperDroid Robots
//			: All Rights Reserved
//  Arduino		: Arduino Uno R3	
//  A Mirf example to use the nRF24L01 and Arduino to act as a wireless remote.
//  The Robot reads data and controls a sabertooth

//  Hardware SPI:
//    MISO -> 12
//    MOSI -> 11
//    SCK -> 13
// 
//  Configurable:
//    CE -> 8
//    CSN -> 7

#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef struct {
 unsigned char prefix;
 unsigned char analog0;
 unsigned char analog1;
 unsigned char analog2;
 unsigned char digital6;
 unsigned char packetChecksum; 
} remoteToRobot;

SoftwareSerial SWSerialB(NOT_A_PIN, 3); // TX on pin 3 (to S1).
Sabertooth ST_back(128, SWSerialB); // Address 128, and use SWSerial as the serial port.

SoftwareSerial SWSerialF(NOT_A_PIN, 5); // TX on pin 5 (to S2).
Sabertooth ST_front(129, SWSerialF); // Address 128, and use SWSerial as the serial port.

//connection loss timeout
volatile int rxTimeoutCounter = 0;
int rxTimeoutTime = 10;	//1 second timeout on 10 Hz timer

void initTimer()
{
	// initialize timer1 with 10 Hz frequnecy
	noInterrupts();           // disable all interrupts
	TCCR1A = 0;
	TCCR1B = 0;
	TCNT1 = 0;

	//OCR1A = 31250;            // compare match register 16MHz/256/2Hz
	OCR1A = 6250;            // compare match register 16MHz/256/10Hz
	TCCR1B |= (1 << WGM12);   // CTC mode
	TCCR1B |= (1 << CS12);    // 256 prescaler 
	TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
	interrupts();             // enable all interrupts
}

// timer1 interrupt
ISR(TIMER1_COMPA_vect)          // timer compare interrupt service routine
{
	rxTimeoutCounter++;
	if(rxTimeoutCounter > 10) { rxTimeoutCounter = 10; }

}

void setup()
{
  SWSerialF.begin(9600);
  SWSerialB.begin(9600);
  Serial.begin(9600);
  ST_front.autobaud();
  ST_back.autobaud();
  ST_front.autobaud();
  ST_back.autobaud();
  //Serial.println("Start");
  
  //Set the SPI Driver.
  Mirf.spi = &MirfHardwareSpi;

  //Setup pins / SPI.
  Mirf.init();

  //Configure reciving address.
  Mirf.setRADDR((byte *)"serv1");
  
  //Set payload on client and server must be the same.
  Mirf.payload = 6;
  
  //Write channel and payload config then power up reciver
  Mirf.config();
  
  //Serial.println("Listening..."); 
  initTimer();
}

void loop()
{
  // A buffer to store the data.
  byte data[6];
  remoteToRobot rxPacket;
  unsigned long tempData = 0;
  int Motor1 = 0;
  int Motor2 = 0;
  int Motor3 = 0;
    
   // If a packet has been recived.
   // isSending also restores listening mode when it 
   // transitions from true to false.
   
   if(!Mirf.isSending() && Mirf.dataReady())
   {
      Serial.print("Got packet"); Serial.print(" ");
      
      // Get load the packet into the buffer. 
      Mirf.getData(data);
      
      // Set the send address.
      Mirf.setTADDR((byte *)"clie1");
      
      //Send the data back  
      // This is just a test, send sensor data back, etc.  
      Mirf.send(data);
      
      rxPacket.packetChecksum = data[5];
      rxPacket.digital6 = data[4];
      rxPacket.analog2 = data[3];
      rxPacket.analog1 = data[2];
      rxPacket.analog0 = data[1];
      rxPacket.prefix = data[0];
      
      rxTimeoutCounter=0;
      Serial.print(rxTimeoutCounter); Serial.print(" ");
  
      //Set Motor Values
      Motor1 = rxPacket.analog0;
      if(Motor1 > 120 && Motor1 < 135) Motor1 = 127;
      if(Motor1 > 254) Motor1 = 254;
      if(Motor1 < 0) Motor1 = 0;
      Motor1 = Motor1 - 127;
      Motor2 = rxPacket.analog1;
      if(Motor2 > 120 && Motor2 < 135) Motor2=127;
      if(Motor2 > 254) Motor2 = 254;
      if(Motor2 < 0) Motor2 = 0;
      Motor2 = Motor2 - 127;
       Motor3 = rxPacket.analog2;
      if(Motor3 > 120 && Motor3 < 135) Motor3=127;
      if(Motor3 > 254) Motor3 = 254;
      if(Motor3 < 0) Motor3 = 0;
      Motor3 = Motor3 - 127;
      
      Serial.print(Motor1); Serial.print(" ");
      Serial.print(Motor2); Serial.print(" ");
      Serial.println(Motor3);
      

      //Command Motors Tank
//      ST_front.drive(Motor1);
//      ST_front.turn(Motor2);
//      ST_back.drive(-Motor1);
//      ST_back.turn(Motor2);
      
      //Command Motors Vectoring/Tank
      ST_front.drive(-Motor1);
      ST_front.turn(Motor3-Motor2);
      ST_back.drive(Motor1);
      ST_back.turn(Motor3+Motor2);
  }
  else {  Serial.print("No data"); Serial.print(" ");

// Check for rx timeout
  checkRxTimeout();
  Serial.print(rxTimeoutCounter); Serial.print(" ");
  Serial.print(Motor1); Serial.print(" ");
  Serial.print(Motor2); Serial.print(" ");
  Serial.println(Motor3);}
}

void checkRxTimeout()
{
  if (rxTimeoutCounter >= rxTimeoutTime) {
    // stop motors
    ST_front.drive(0);
    ST_front.turn(0);
    ST_back.drive(0);
    ST_back.turn(0);
  }
}
0 Comments

Leave a reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.

©2020 SDRobots.com | All rights reserved.

Log in with your credentials

or    

Forgot your details?

Create Account