Subversion Repositories group.electronics

/* 
 433Mhz Reviever Connections
     View from front
  __________________________
 |                          |
 |_A_G_G_V__________V_D_D_G_|
   | | | |          | | | |
                    ^ ^   ^
                   5V D2  Gnd 

 MotorShield Connections
 
    M1___G__M2_
   |o|o||o||o|o|
    ^ ^     ^ ^
    | |     | |
   +R -R   -L +L
 R = Right motor, L = Left motor
*/

#include <VirtualWire.h>
#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_2KHZ);  //Right
AF_DCMotor motor2(2, MOTOR12_2KHZ);  //Left

const int rheIntPin = 0;
const int ledPin = 13;

const int  rxPin    = 2;
const int  rxBps    = 2000;

const int  MRIGHT   = 0;
const int  MLEFT    = 1;

const int  MFORWARD = 1;
const int  MREVERSE = 0;

int options = 0;         // Options
int mspd[2] = {0, 0};    //Speed of right track


volatile byte rpmcount = 0;
unsigned int rpm = 0;
unsigned long timeold = 0;

void setup()
{  
    
    Serial.begin(9600); // Debugging only
    Serial.println("setup");

    pinMode(ledPin, OUTPUT);
    digitalWrite(ledPin, false);

    // Startup radio receiver
    vw_set_rx_pin(rxPin);
    vw_setup(rxBps);     // Bits per sec
    vw_rx_start();       // Start the receiver PLL running
    
    // Hall effect intterupt setup
    //pinMode(rhePin, INPUT);
    attachInterrupt(rheIntPin, rpm_rhe, FALLING);
        
   // Hall effect debugging
   //mspd[MRIGHT] = 50;   

   update_motors();   
}

void loop()
{
    uint8_t buf[VW_MAX_MESSAGE_LEN];
    uint8_t buflen = VW_MAX_MESSAGE_LEN;

    if (vw_get_message(buf, &buflen)) // Non-blocking
    {
        // Flash on msg get
        //digitalWrite(13, true);
        //digitalWrite(13, false);
        /*
        int i;
        Serial.print("Got: ");
        for (i = 0; i < buflen; i++)
        {
            Serial.print(buf[i], HEX);
            Serial.print(" ");
        }
        Serial.println("");
        */


        
        if (buf[0])      // Read the options
          options = buf[0] - 1;
   
       if (buf[1])      // Right speed
          mspd[MRIGHT] = buf[1] -1;     
          
       if (buf[2])      // Left speed
          mspd[MLEFT] = buf[2] -1;
               
       
       Serial.print("options "); Serial.print(options, BIN);
       Serial.print(" r_spd "); Serial.print(mspd[MRIGHT], DEC);
       Serial.print(" l_spd "); Serial.print(mspd[MLEFT], DEC);
       Serial.println(""); 

       update_motors();       
    }
    
    if (rpmcount >= 5) {
      rpm = 30*1000/(millis() - timeold)*rpmcount;
      timeold = millis();
      rpmcount = 0;
      Serial.println(rpm,DEC);
    }
}

void rpm_rhe() {
  rpmcount++;
  digitalWrite(13, true);
  digitalWrite(13, false);
}

void update_motors() {       
       // Start the tracks rolling
       if (mspd[MRIGHT] > 0) {
         motor1.setSpeed(mspd[MRIGHT]);
         if (bitRead(options, MRIGHT))
           motor1.run(FORWARD);
         else
           motor1.run(BACKWARD);
       } else
         motor1.run(RELEASE);
            
       if (mspd[MLEFT] > 0) {
         motor2.setSpeed(mspd[MLEFT]);
         if (bitRead(options, MLEFT))
           motor2.run(FORWARD);
         else
           motor2.run(BACKWARD);
       } else
         motor2.run(RELEASE);
}