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); } }
