//
// File:   rc_motor.v
// Date:   28-Oct-05
// Author: I. Chuang <ichuang@mit.edu>
//
// RC servo motor driver written in Verilog.
//
// The main module to drive a motor is rc_motor_move.  
//
// Inputs:
//
//    clock_27mhz   - synchronous clock
//    position      - 8-bit value specifying desired motor position
//                    For the parallax servo, this should be > 0x38 and < 0xf8
// Output:
//
//    motor_out     - wire output driving motor
//
// This module uses rc_motor, a lower level module which allows
// individual pulse timing to be controlled.
//
// The servo motor is driven by a single line, which should have a single
// pulse of duration pw every interval seconds, where pw determines the
// position to which the motor should move.  Multiple pulses should be sent
// to drive the motor to its final position.  rc_motor_move sends 16 pulses.

module rc_motor_move(clock_27mhz, position, motor_out);
   input clock_27mhz;
   input [7:0] position;
   output motor_out;

   wire cycle;
   reg oldpos;
   reg [3:0] gocount;
   wire go = (gocount > 0) ? 1 : 0;  // pulse motor 16 times each time pos changes

   rc_motor rcm1(clock_27mhz, {4'b0,position}, 12'h500, go, motor_out, cycle);

   always @(posedge clock_27mhz) begin
   	oldpos <= cycle ? position : oldpos;  // record new pos request each cycle
     gocount <= (oldpos != position) ? 1 :   // enable go when pos changes
					( ((gocount>1)&cycle) ? gocount + 1 : gocount );
   end		

endmodule


module rc_motor(clock_27mhz, pulsewidth, interval, go, motor_out, motor_cycle);
   
   input       clock_27mhz;
   input [11:0] pulsewidth;
   input [11:0] interval;
   input       go;
   output      motor_out, motor_cycle;

   parameter PW_CLOCK = 270;
 
   // the RC servo motor works by taking in a pulse with width from ~0.6ms
   // to ~2.5ms and translating that to a motor position.  Send a sequence
   // of pulses spaced by ~10 ms to keep driving the motor to the final 
   // position.

   // generate 10 us clock (pulse high for one cycle of clock_27mhz)
   reg [16:0]  count;
   wire        tkhz = (count==PW_CLOCK);
   always @(posedge clock_27mhz) count <= tkhz ? 0 : count+1;
   
   // generate pulse counter (zero to PW_INTERVAL)
   reg [11:0]  pulsec;
   reg 	       go_latched;
   wire motor_cycle = (pulsec == interval);
   always @(posedge clock_27mhz) begin
      pulsec <= ~tkhz ? pulsec : ( motor_cycle ? 0 : pulsec+1 );
      go_latched <= motor_cycle ? go : go_latched;
   end

   // generate motor_out pulse
   wire        motor_out = (pulsec < pulsewidth) ? go_latched : 0;

endmodule // rc_motor