Skip to content

Commit

Permalink
considering back emf to be done for now, fixed sign issue
Browse files Browse the repository at this point in the history
  • Loading branch information
Josh S committed Dec 22, 2012
1 parent f50d815 commit 0e141a6
Show file tree
Hide file tree
Showing 13 changed files with 1,760 additions and 63 deletions.
18 changes: 9 additions & 9 deletions auto_adc_updater.v
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ module auto_adc_updater(
input clk3p2M,
input [9:0] adc_in,
input adc_valid,
//input bemf_sensing,
input bemf_sensing,
output adc_go,
output [3:0] adc_chan,
output [9:0] adc_0_in,
Expand Down Expand Up @@ -125,14 +125,14 @@ module auto_adc_updater(
5'd5: adc_5_in_r <= adc_in;
5'd6: adc_6_in_r <= adc_in;
5'd7: adc_7_in_r <= adc_in;
5'd8: adc_8_in_r <= adc_in;
5'd9: adc_9_in_r <= adc_in;
5'd10: adc_10_in_r <= adc_in;
5'd11: adc_11_in_r <= adc_in;
5'd12: adc_12_in_r <= adc_in;
5'd13: adc_13_in_r <= adc_in;
5'd14: adc_14_in_r <= adc_in;
5'd15: adc_15_in_r <= adc_in;
5'd8: if (bemf_sensing) adc_8_in_r <= adc_in;
5'd9: if (bemf_sensing) adc_9_in_r <= adc_in;
5'd10: if (bemf_sensing) adc_10_in_r <= adc_in;
5'd11: if (bemf_sensing) adc_11_in_r <= adc_in;
5'd12: if (bemf_sensing) adc_12_in_r <= adc_in;
5'd13: if (bemf_sensing) adc_13_in_r <= adc_in;
5'd14: if (bemf_sensing) adc_14_in_r <= adc_in;
5'd15: if (bemf_sensing) adc_15_in_r <= adc_in;
5'd16: adc_16_in_r <= adc_in;
default:;
endcase
Expand Down
49 changes: 31 additions & 18 deletions bemf_update.v
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,10 @@ module bemf_update(
input [9:0] bemf_adc_l,
input [1:0] mot_sel_in,
input in_valid,
input [15:0] bemf_in,
input [15:0] bemf_calib_in,
input [21:0] bemf_in,
input [21:0] bemf_calib_in,
input clk,
output [15:0] bemf_out,
output [21:0] bemf_out,
output [1:0] mot_sel_out,
output out_valid
);
Expand All @@ -17,30 +17,30 @@ module bemf_update(
// subtract the high and low side of the motor
reg [9:0] bemf_sub_in_a_r = 10'd0;
reg [9:0] bemf_sub_in_b_r = 10'd0;
wire [15:0] bemf_sub_out;
s16_sub bemf_sub(
.a({6'd0, bemf_sub_in_a_r}),
.b({6'd0, bemf_sub_in_b_r}),
wire [21:0] bemf_sub_out;
s22_sub bemf_sub(
.a({16'd0, bemf_sub_in_a_r}),
.b({16'd0, bemf_sub_in_b_r}),
.s(bemf_sub_out)
);


// adjust by the calibration amount
reg [15:0] calib_sub_in_a_r = 16'd0;
reg [15:0] calib_sub_in_b_r = 16'd0;
wire [15:0] calib_sub_out;
s16_sub calib_sub(
reg [21:0] calib_sub_in_a_r = 22'd0;
reg [21:0] calib_sub_in_b_r = 22'd0;
wire [21:0] calib_sub_out;
s22_sub calib_sub(
.a(calib_sub_in_a_r),
.b(calib_sub_in_b_r),
.s(calib_sub_out)
);


// integrate the calibrated back emf readings
reg [15:0] bemf_integr_in_a_r = 16'd0;
reg [15:0] bemf_integr_in_b_r = 16'd0;
wire [15:0] bemf_integr_out;
s16_add bemf_integr(
reg [21:0] bemf_integr_in_a_r = 22'd0;
reg [21:0] bemf_integr_in_b_r = 22'd0;
wire [21:0] bemf_integr_out;
s22_add bemf_integr(
.a(bemf_integr_in_a_r),
.b(bemf_integr_in_b_r),
.s(bemf_integr_out)
Expand All @@ -57,7 +57,10 @@ module bemf_update(
reg in_valid_2 = 1'd0;
reg in_valid_3 = 1'd0;

reg [15:0] bemf_out_r = 16'd0;
reg [21:0] bemf_out_r = 22'd0;

reg [21:0] bemf_in_0 = 22'd0;
reg [21:0] bemf_in_1 = 22'd0;

always @ (posedge clk) begin

Expand All @@ -66,18 +69,28 @@ module bemf_update(
bemf_sub_in_b_r <= bemf_adc_l;
in_valid_0 <= in_valid;
mot_sel_in_0 <= mot_sel_in;
bemf_in_0 <= bemf_in;


// pipeline stage 1: calculated (bemf_high - bemf_low)
calib_sub_in_a_r <= bemf_sub_out;
calib_sub_in_b_r <= bemf_calib_in;
in_valid_1 <= in_valid_0;
mot_sel_in_1 <= mot_sel_in_0;
bemf_in_1 <= bemf_in_0;



// pipeline stage 2: subtracted the calibration info
bemf_integr_in_a_r <= calib_sub_out;
bemf_integr_in_b_r <= bemf_in;

// deadbanding
if (calib_sub_out[21] == 1'b1)
bemf_integr_in_a_r <= (~calib_sub_out > 22'd21) ? calib_sub_out : 22'd0; //negative
else
bemf_integr_in_a_r <= (calib_sub_out > 22'd20) ? calib_sub_out : 22'd0; // positive

//bemf_integr_in_a_r <= calib_sub_out;
bemf_integr_in_b_r <= bemf_in_1;
in_valid_2 <= in_valid_1;
mot_sel_in_2 <= mot_sel_in_1;

Expand Down
Binary file modified kovan.bit
View file
Binary file not shown.
77 changes: 43 additions & 34 deletions kovan.v
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,26 +312,26 @@ module kovan (
reg [9:0] adc_15_new;
reg [9:0] adc_16_new;

wire [15:0] bemf_0;
wire [15:0] bemf_1;
wire [15:0] bemf_2;
wire [15:0] bemf_3;
wire [21:0] bemf_0;
wire [21:0] bemf_1;
wire [21:0] bemf_2;
wire [21:0] bemf_3;

reg [15:0] bemf_0_r;
reg [15:0] bemf_1_r;
reg [15:0] bemf_2_r;
reg [15:0] bemf_3_r;
reg [21:0] bemf_0_r;
reg [21:0] bemf_1_r;
reg [21:0] bemf_2_r;
reg [21:0] bemf_3_r;


reg [15:0] bemf_0_r_208M = 16'd0;
reg [15:0] bemf_1_r_208M = 16'd0;
reg [15:0] bemf_2_r_208M = 16'd0;
reg [15:0] bemf_3_r_208M = 16'd0;
reg [21:0] bemf_0_r_208M = 22'd0;
reg [21:0] bemf_1_r_208M = 22'd0;
reg [21:0] bemf_2_r_208M = 22'd0;
reg [21:0] bemf_3_r_208M = 22'd0;

reg [15:0] bemf_0_calib = 16'd0;
reg [15:0] bemf_1_calib = 16'd0;
reg [15:0] bemf_2_calib = 16'd0;
reg [15:0] bemf_3_calib = 16'd0;
reg [21:0] bemf_0_calib = 22'd0;
reg [21:0] bemf_1_calib = 22'd0;
reg [21:0] bemf_2_calib = 22'd0;
reg [21:0] bemf_3_calib = 22'd0;


reg [11:0] mot_duty0_old = 12'd0;
Expand Down Expand Up @@ -424,6 +424,9 @@ module kovan (
assign bemf_2 = bemf_2_r_208M;
assign bemf_3 = bemf_3_r_208M;

wire bemf_sensing;
reg bemf_sensing_r = 1'd0;
assign bemf_sensing = bemf_sensing_r;

always @ (posedge clk208M) begin

Expand Down Expand Up @@ -503,12 +506,12 @@ module kovan (
reg [9:0] bemf_adc_l = 10'd0;
reg [1:0] bemf_mot_sel_in = 2'd0;
reg bemf_in_valid = 1'd0;
reg [15:0] bemf_in = 16'd0;
reg [15:0] bemf_calib_in = 16'd0;
reg [21:0] bemf_in = 22'd0;
reg [21:0] bemf_calib_in = 22'd0;

wire [1:0] bemf_mot_sel_out;
wire bemf_out_valid;
wire [15:0] bemf_out;
wire [21:0] bemf_out;

reg [15:0] bemf_counter = 16'd0; // can count for up to ~20mS
reg [2:0] bemf_state = 3'd0;
Expand Down Expand Up @@ -552,6 +555,7 @@ module kovan (
3'd0: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_in_valid <= 1'd0;
bemf_sensing_r <= 1'd0;

if (bemf_counter > 3200) // 1mS
bemf_state <= bemf_state + 1'd1;
Expand All @@ -563,7 +567,9 @@ module kovan (
3'd1: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_in_valid <= 1'd0;
if (bemf_counter > 6400) // 2mS
bemf_sensing_r <= 1'd1;

if (bemf_counter > 12800) // 4mS
bemf_state <= bemf_state + 1'd1;
else
bemf_state <= bemf_state;
Expand All @@ -573,11 +579,12 @@ module kovan (
3'd2: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_state <= bemf_state + 1'd1;
bemf_adc_h <= adc_8_in;
bemf_adc_l <= adc_9_in;
bemf_adc_h <= adc_9_in;
bemf_adc_l <= adc_8_in;
bemf_mot_sel_in <= 2'd0;
bemf_in_valid <= 1'd1;
bemf_in <= bemf_0;
bemf_sensing_r <= 1'd0;
bemf_calib_in <= bemf_0_calib;
end

Expand All @@ -586,8 +593,8 @@ module kovan (
3'd3: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_state <= bemf_state + 1'd1;
bemf_adc_h <= adc_10_in;
bemf_adc_l <= adc_11_in;
bemf_adc_h <= adc_11_in;
bemf_adc_l <= adc_10_in;
bemf_mot_sel_in <= 2'd1;
bemf_in_valid <= 1'd1;
bemf_in <= bemf_1;
Expand All @@ -599,8 +606,8 @@ module kovan (
3'd4: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_state <= bemf_state + 1'd1;
bemf_adc_h <= adc_12_in;
bemf_adc_l <= adc_13_in;
bemf_adc_h <= adc_13_in;
bemf_adc_l <= adc_12_in;
bemf_mot_sel_in <= 2'd2;
bemf_in_valid <= 1'd1;
bemf_in <= bemf_2;
Expand All @@ -612,8 +619,8 @@ module kovan (
3'd5: begin
bemf_counter <= bemf_counter + 1'd1;
bemf_state <= bemf_state + 1'd1;
bemf_adc_h <= adc_14_in;
bemf_adc_l <= adc_15_in;
bemf_adc_h <= adc_15_in;
bemf_adc_l <= adc_14_in;
bemf_mot_sel_in <= 2'd3;
bemf_in_valid <= 1'd1;
bemf_in <= bemf_3;
Expand All @@ -625,7 +632,7 @@ module kovan (
3'd6: begin
bemf_in_valid <= 1'd0;

if (bemf_counter > 32000) begin// 10mS total
if (bemf_counter > 64000) begin// 20mS total
bemf_state <= bemf_state + 1'd1;
bemf_counter <= bemf_counter + 1'd1;
end else begin
Expand All @@ -636,6 +643,7 @@ module kovan (

default: begin
bemf_state <= 3'd0;
bemf_sensing_r <= 1'd0;
bemf_counter <= 16'd0;
end
endcase
Expand Down Expand Up @@ -685,10 +693,10 @@ module kovan (
.adc_15_in(adc_15_old),
.adc_16_in(adc_16_old),
.charge_acp_in(CHG_ACP),
.bemf_0(bemf_0_r_208M),
.bemf_1(bemf_1_r_208M),
.bemf_2(bemf_2_r_208M),
.bemf_3(bemf_3_r_208M),
.bemf_0(bemf_0_r_208M[21:6]),
.bemf_1(bemf_1_r_208M[21:6]),
.bemf_2(bemf_2_r_208M[21:6]),
.bemf_3(bemf_3_r_208M[21:6]),
.servo_pwm0_high(servo_pwm0_old),
.servo_pwm1_high(servo_pwm1_old),
.servo_pwm2_high(servo_pwm2_old),
Expand Down Expand Up @@ -775,6 +783,7 @@ module kovan (
.duty2(mot_duty2),
.duty3(mot_duty3),
.drive_code(mot_drive_code),
.bemf_sensing(bemf_sensing),
.pwm(MOT_PWM),
.MBOT(MBOT[3:0]),
.MTOP(MTOP[3:0])
Expand All @@ -784,7 +793,7 @@ module kovan (
.clk3p2M(clk3p2M),
.adc_in(adc_in),
.adc_valid(adc_valid),
//.bemf_sensing(bemf_sensing),
.bemf_sensing(bemf_sensing),
.adc_go(adc_go),
.adc_chan(adc_chan),
.adc_0_in(adc_0_in),
Expand Down
5 changes: 3 additions & 2 deletions quad_motor.v
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ module quad_motor(
input [11:0] duty2,
input [11:0] duty3,
input [7:0] drive_code,
input bemf_sensing,
output pwm,
output [3:0] MBOT,
output [3:0] MTOP
Expand All @@ -41,8 +42,8 @@ module quad_motor(
reg [3:0] MTOP_r = 4'b0000;
reg [11:0] count = 12'h0000;

assign MBOT[3:0] = MTOP_r[3:0];
assign MTOP[3:0] = MBOT_r[3:0];
assign MBOT[3:0] = MTOP_r[3:0] & {~bemf_sensing,~bemf_sensing,~bemf_sensing,~bemf_sensing};
assign MTOP[3:0] = MBOT_r[3:0] & {~bemf_sensing,~bemf_sensing,~bemf_sensing,~bemf_sensing};

reg pwm_r;
assign pwm = pwm_r;
Expand Down
Loading

0 comments on commit 0e141a6

Please sign in to comment.