Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

pid is mostly connected

  • Loading branch information...
commit f88902bd7011a0e27759b9da43055c9b8e4aeaf1 1 parent 81d9a0d
Josh S authored
Showing with 229 additions and 90 deletions.
  1. +7 −0 bemf_update.v
  2. BIN  kovan.bit
  3. +161 −80 kovan.v
  4. +39 −5 pid.v
  5. +22 −5 spi.v
View
7 bemf_update.v
@@ -9,6 +9,7 @@ module bemf_update(
input [19:0] bemf_calib_in,
input clk,
output [19:0] bemf_out,
+ output [19:0] bemf_vel_out,
output [1:0] mot_sel_out,
output out_valid
);
@@ -62,6 +63,9 @@ module bemf_update(
reg [19:0] bemf_in_0 = 20'd0;
reg [19:0] bemf_in_1 = 20'd0;
+
+ reg vel_out_3 = 20'd0;
+
always @ (posedge clk) begin
// pipeline stage 0: load inputs
@@ -96,6 +100,8 @@ module bemf_update(
// pipeline stage 3: integrated the calibrated value
+
+ vel_out_3 <= bemf_integr_in_a_r;
bemf_out_r <= bemf_integr_out;
in_valid_3 <= in_valid_2;
mot_sel_in_3 <= mot_sel_in_2;
@@ -105,5 +111,6 @@ module bemf_update(
assign bemf_out = bemf_out_r;
assign out_valid = in_valid_3;
assign mot_sel_out = mot_sel_in_3;
+ assign bemf_vel_out = vel_out_3;
endmodule
View
BIN  kovan.bit
Binary file not shown
View
241 kovan.v
@@ -35,7 +35,6 @@ module kovan (
input wire CHG_ACP, // reports presence of AC power
//output wire CHG_SHDN, // leave floating
-
// i/o controller digital interfaces
input wire DIG_ADC_OUT,
output wire [1:0] DIG_ADC_CS,
@@ -56,16 +55,6 @@ module kovan (
output wire MOT_PWM,
output wire [3:0] M_SERVO,
- // optional uart to outside world
- //input wire EXT_TO_HOST_UART, // for now we're a fly on the wall
- //input wire HOST_TO_EXT_UART,
-
- // infrared receiver
- //input wire IR_RX,
-
- // switch
- //input wire INPUT_SW0,
-
// audio pass-through
input wire I2S_CDCLK0, // master reference clock to audio PLL
output wire I2S_CDCLK1,
@@ -103,14 +92,7 @@ module kovan (
input wire FPGA_MOSI,
input wire FPGA_SCLK,
input wire FPGA_SYNC,
-
- // I2C interfaces
- //input wire PWR_SCL, // we listen on this one
- //inout wire PWR_SDA,
-
- // LED
- //output wire FPGA_LED,
-
+
output wire ADC_BATT_SEL,
input wire OSC_CLK // 26 mhz clock from CPU
@@ -130,18 +112,6 @@ module kovan (
IBUFG clk26buf_ibuf(.I(clk26), .O(clk26ibuf));
BUFG clk26buf_buf (.I(clk26ibuf), .O(clk26buf));
-
- ////////// reset
- /*
- reg glbl_reset = 1'b0; // to be used sparingly
- //wire glbl_reset_edge = 1'b0;
- //reg glbl_reset_edge_d;
-
- always @(posedge clk1M) begin
- //glbl_reset_edge_d <= 1'b0;//gbl_reset_edge;
- glbl_reset <= 1'b0;//!glbl_reset_edge_d & glbl_reset_edge; // just pulse reset for one cycle of the slowest clock in the system
- end
- */
////////// loop-throughs
// lcd runs at a target of 6.41 MHz (156 ns cycle time)
@@ -382,16 +352,28 @@ module kovan (
reg [11:0] pid_pwm2 = 12'd0;
reg [11:0] pid_pwm3 = 12'd0;
- reg pid_dir0 = 1'd0;
- reg pid_dir1 = 1'd0;
- reg pid_dir2 = 1'd0;
- reg pid_dir3 = 1'd0;
+ reg [1:0] pid_drive_code_0 = 2'd0;
+ reg [1:0] pid_drive_code_1 = 2'd0;
+ reg [1:0] pid_drive_code_2 = 2'd0;
+ reg [1:0] pid_drive_code_3 = 2'd0;
+
+ wire [15:0] pid_p_goal_0_new;
+ wire [15:0] pid_p_goal_1_new;
+ wire [15:0] pid_p_goal_2_new;
+ wire [15:0] pid_p_goal_3_new;
+
+
+ reg [15:0] pid_p_goal_0_old = 16'd0;
+ reg [15:0] pid_p_goal_1_old = 16'd0;
+ reg [15:0] pid_p_goal_2_old = 16'd0;
+ reg [15:0] pid_p_goal_3_old = 16'd0;
+
// TODO: direction, convert to unsigned
- assign mot_duty0 = (mot_duty0_old[15]) ? pid_pwm0[11:0] : mot_duty0_new;
- assign mot_duty1 = (mot_duty1_old[15]) ? pid_pwm1[11:0] : mot_duty1_new;
- assign mot_duty2 = (mot_duty2_old[15]) ? pid_pwm2[11:0] : mot_duty2_new;
- assign mot_duty3 = (mot_duty3_old[15]) ? pid_pwm3[11:0] : mot_duty3_new;
+ assign mot_duty0 = (mot_duty0_old[15]) ? pid_pwm0[11:0] : mot_duty0_new[11:0];
+ assign mot_duty1 = (mot_duty1_old[15]) ? pid_pwm1[11:0] : mot_duty1_new[11:0];
+ assign mot_duty2 = (mot_duty2_old[15]) ? pid_pwm2[11:0] : mot_duty2_new[11:0];
+ assign mot_duty3 = (mot_duty3_old[15]) ? pid_pwm3[11:0] : mot_duty3_new[11:0];
assign servo0_pwm_pulse = {servo_pwm0_old, 8'd0};
assign servo1_pwm_pulse = {servo_pwm1_old, 8'd0};
@@ -408,11 +390,16 @@ module kovan (
assign dig_sample = dig_sample_old;
assign dig_update = dig_update_old;
- //TODO: motor drive code assignment should consider pid mode
- assign mot_drive_code = mot_drive_code_old;
+ assign mot_drive_code[7:6] = (mot_duty0_old[15]) ? pid_drive_code_0 : mot_drive_code_old[7:6];
+ assign mot_drive_code[5:4] = (mot_duty1_old[15]) ? pid_drive_code_1 : mot_drive_code_old[5:4];
+ assign mot_drive_code[3:2] = (mot_duty2_old[15]) ? pid_drive_code_2 : mot_drive_code_old[3:2];
+ assign mot_drive_code[1:0] = (mot_duty3_old[15]) ? pid_drive_code_3 : mot_drive_code_old[1:0];
assign mot_allstop = mot_allstop_old;
+ reg [3:0] pid_at_goal = 4'd0;
+ reg [3:0] pid_at_goal_old = 4'd0;
+
assign bemf_0 = bemf_0_r_208M;
assign bemf_1 = bemf_1_r_208M;
@@ -489,13 +476,22 @@ module kovan (
dig_in_val_new <= dig_in_val;
dig_in_val_old <= dig_in_val_new;
+
+ pid_at_goal_old <= pid_at_goal;
+ pid_p_goal_0_old <= pid_p_goal_0_new;
+ pid_p_goal_1_old <= pid_p_goal_1_new;
+ pid_p_goal_2_old <= pid_p_goal_2_new;
+ pid_p_goal_3_old <= pid_p_goal_3_new;
end
assign servo_pwm_period[23:0] = 24'h03F7A0;
-
-
+ wire [19:0] bemf_vel_out;
+ reg [19:0] vel_mot_0 = 20'd0;
+ reg [19:0] vel_mot_1 = 20'd0;
+ reg [19:0] vel_mot_2 = 20'd0;
+ reg [19:0] vel_mot_3 = 20'd0;
reg [9:0] bemf_adc_h = 10'd0;
reg [9:0] bemf_adc_l = 10'd0;
@@ -518,15 +514,19 @@ module kovan (
case(bemf_mot_sel_out)
2'd0: begin
bemf_0_r <= bemf_out;
+ vel_mot_0 <= bemf_vel_out;
end
2'd1: begin
bemf_1_r <= bemf_out;
+ vel_mot_1 <= bemf_vel_out;
end
2'd2: begin
bemf_2_r <= bemf_out;
+ vel_mot_2 <= bemf_vel_out;
end
2'd3: begin
bemf_3_r <= bemf_out;
+ vel_mot_3 <= bemf_vel_out;
end
endcase
end
@@ -642,6 +642,7 @@ module kovan (
.bemf_calib_in(bemf_calib_in),
.clk(clk3p2M),
.bemf_out(bemf_out),
+ .bemf_vel_out(bemf_vel_out),
.mot_sel_out(bemf_mot_sel_out),
.out_valid(bemf_out_valid)
);
@@ -697,9 +698,12 @@ module kovan (
.dig_update(dig_update_old),
.mot_drive_code(mot_drive_code_old),
.mot_allstop(mot_allstop_old),
- // .bemf_calib_cmd(bemf_calib_cmd_old),
-
- // Read-Write Registers
+ .pid_p_goal_0(pid_p_goal_0_old),
+ .pid_p_goal_1(pid_p_goal_1_old),
+ .pid_p_goal_2(pid_p_goal_2_old),
+ .pid_p_goal_3(pid_p_goal_3_old),
+ .pid_at_goal(pid_at_goal_old),
+
.servo_pwm0_high_new(servo_pwm0_new),
.servo_pwm1_high_new(servo_pwm1_new),
.servo_pwm2_high_new(servo_pwm2_new),
@@ -715,8 +719,11 @@ module kovan (
.dig_sample_new(dig_sample_new),
.dig_update_new(dig_update_new),
.mot_drive_code_new(mot_drive_code_new),
- .mot_allstop_new(mot_allstop_new)
- // .bemf_calib_cmd_new(bemf_calib_cmd_new)
+ .mot_allstop_new(mot_allstop_new),
+ .pid_p_goal_0_new(pid_p_goal_0_new),
+ .pid_p_goal_1_new(pid_p_goal_1_new),
+ .pid_p_goal_2_new(pid_p_goal_2_new),
+ .pid_p_goal_3_new(pid_p_goal_3_new)
);
@@ -727,21 +734,30 @@ module kovan (
reg [12:0] pos = 13'd0;
reg [12:0] err_prev = 13'd0;
reg [12:0] int_err_prev = 13'd0;
- reg [12:0] Kp_n = 13'd0;
- reg [7:0] Kp_d = 13'd0;
+ reg [12:0] Kp_n = 13'd10;
+ reg [7:0] Kp_d = 8'd0;
reg [12:0] Ki_n = 13'd0;
- reg [7:0] Ki_d = 13'd0;
+ reg [7:0] Ki_d = 8'd0;
reg [12:0] Kd_n = 13'd0;
- reg [7:0] Kd_d = 13'd0;
+ reg [7:0] Kd_d = 8'd0;
+
+ reg [1:0] pid_motor_id_in = 2'd0;
+ reg pid_in_valid = 1'd0;
// Outputs
- wire [12:0] pid_pwm;
+ wire [11:0] pid_pwm;
wire [12:0] pid_err;
wire [12:0] pid_int_err;
- wire pid_dir;
+ wire [1:0] pid_drive_code;
+ wire [1:0] pid_motor_id_out;
+ wire pid_out_valid;
+
+ reg [12:0] vel_d;
+ reg [12:0] vel;
- // Inst/antiate the Unit Under Test (UUT)
pid pid_6stage (
+ .vel_d(vel_d),
+ .vel(vel),
.pos_d(pos_d),
.pos(pos),
.err_prev(err_prev),
@@ -751,25 +767,104 @@ module kovan (
.Ki_n(Ki_n),
.Ki_d(Ki_d),
.Kd_n(Kd_n),
- .Kd_d(Kd_d),
+ .Kd_d(Kd_d),
+ .pid_motor_id_in(pid_motor_id_in),
+ .pid_in_valid(pid_in_valid),
.clk(clk3p2M),
.pwm(pid_pwm),
.err(pid_err),
.int_err(pid_int_err),
- .dir(pid_dir)
+ .drive_code(pid_drive_code),
+ .pid_motor_id_out(pid_motor_id_out),
+ .pid_out_valid(pid_out_valid)
);
-
+ reg [2:0] pid_update_state = 3'd0;
always @(posedge clk3p2M) begin
- pid_pwm0 <= pid_pwm;
- pid_pwm1 <= pid_pwm;
- pid_pwm2 <= pid_pwm;
- pid_pwm3 <= pid_pwm;
- pid_dir0 <= pid_dir;
- pid_dir1 <= pid_dir;
- pid_dir2 <= pid_dir;
- pid_dir3 <= pid_dir;
+
+ pid_at_goal[0] <= pid_drive_code_0 == 2'b00;
+ pid_at_goal[1] <= pid_drive_code_1 == 2'b00;
+ pid_at_goal[2] <= pid_drive_code_2 == 2'b00;
+ pid_at_goal[3] <= pid_drive_code_3 == 2'b00;
+
+ case(pid_update_state)
+ // state 0: clock in mot 0
+ 3'd0: begin
+ pid_in_valid <= 1'b1;
+ pid_motor_id_in <= 2'd0;
+ vel <= vel_mot_0[19:7];
+ vel_d <= mot_duty0_old[12:0];
+ pos <= bemf_0[19:7];
+ pos_d <= pid_p_goal_0_old[12:0];
+ pid_update_state <= pid_update_state + 1'b1;
+ end
+ // state 1: clock in mot 1
+ 3'd1: begin
+ pid_in_valid <= 1'b1;
+ pid_motor_id_in <= 2'd1;
+ vel <= vel_mot_1[19:7];
+ vel_d <= mot_duty1_old[12:0];
+ pos <= bemf_1[19:7];
+ pos_d <= pid_p_goal_1_old[12:0];
+ pid_update_state <= pid_update_state + 1'b1;
+ end
+ // state 2: clock in mot 2
+ 3'd2: begin
+ pid_in_valid <= 1'b1;
+ pid_motor_id_in <= 2'd2;
+ vel <= vel_mot_2[19:7];
+ vel_d <= mot_duty2_old[12:0];
+ pos <= bemf_2[19:7];
+ pos_d <= pid_p_goal_2_old[12:0];
+ pid_update_state <= pid_update_state + 1'b1;
+ end
+ // state 3: clock in mot 3
+ 3'd3: begin
+ pid_in_valid <= 1'b1;
+ pid_motor_id_in <= 2'd3;
+ vel <= vel_mot_3[19:7];
+ vel_d <= mot_duty3_old[12:0];
+ pos <= bemf_3[19:7];
+ pos_d <= pid_p_goal_3_old[12:0];
+ pid_update_state <= pid_update_state + 1'b1;
+ end
+ // state 4: wait for updates
+ 3'd4: begin
+ pid_in_valid <= 1'b0;
+
+ if (pid_out_valid) begin
+ case(pid_motor_id_out)
+ 2'd0: begin
+ //TODO: err, vel out?
+ pid_drive_code_0 <= pid_drive_code;
+ pid_pwm0 <= pid_pwm;
+ pid_update_state <= 3'd4;
+ end
+ 2'd1: begin
+ pid_drive_code_1 <= pid_drive_code;
+ pid_pwm1 <= pid_pwm;
+ pid_update_state <= 3'd4;
+ end
+ 2'd2: begin
+ pid_drive_code_2 <= pid_drive_code;
+ pid_pwm2 <= pid_pwm;
+ pid_update_state <= 3'd4;
+ end
+ 2'd3: begin
+ pid_drive_code_3 <= pid_drive_code;
+ pid_pwm3 <= pid_pwm;
+ pid_update_state <= 3'd0;
+ end
+ endcase
+ end
+ end
+ // default: go to state 0
+ default: begin
+ pid_in_valid <= 1'b0;
+ pid_update_state <= 3'd0;
+ end
+ endcase
end
@@ -891,19 +986,5 @@ module kovan (
.DIG_SRLOAD(DIG_SRLOAD),
.DIG_CLR_N(DIG_CLR_N)
);
-
-
- /*
- // TODO:
- pwm heartbeat(
- .clk812k(clk1M),
- .pwmout(blue_led),
- .bright(12'b0001_1111_1000),
- .dim(12'b0000_0000_1000)
- );
- assign FPGA_LED = !blue_led;
-*/
-
-
endmodule // kovan
View
44 pid.v
@@ -13,11 +13,15 @@ module pid(
input [7:0] Ki_d,
input [12:0] Kd_n,
input [7:0] Kd_d,
+ input [1:0] pid_motor_id_in,
+ input pid_in_valid,
input clk,
- output [12:0] pwm,
+ output [11:0] pwm,
output [12:0] err,
output [12:0] int_err,
- output dir
+ output [1:0] drive_code,
+ output [1:0] pid_motor_id_out,
+ output pid_out_valid
);
@@ -181,7 +185,21 @@ module pid(
reg close_enough_1;
reg close_enough_2;
+ reg [1:0] pid_motor_id_in_0;
+ reg [1:0] pid_motor_id_in_1;
+ reg [1:0] pid_motor_id_in_2;
+ reg [1:0] pid_motor_id_in_3;
+ reg [1:0] pid_motor_id_in_4;
+ reg [1:0] pid_motor_id_in_5;
+ reg pid_in_valid_0 = 1'b0;
+ reg pid_in_valid_1 = 1'b0;
+ reg pid_in_valid_2 = 1'b0;
+ reg pid_in_valid_3 = 1'b0;
+ reg pid_in_valid_4 = 1'b0;
+ reg pid_in_valid_5 = 1'b0;
+
+
always @ (posedge clk) begin
@@ -200,6 +218,10 @@ module pid(
Kp_d_0 <= Kp_d;
Ki_d_0 <= Ki_d;
Kd_d_0 <= Kd_d;
+
+ pid_motor_id_in_0 <= pid_motor_id_in;
+ pid_in_valid_0 <= pid_in_valid;
+
// pipeline stage 1: calculate position errors
@@ -220,6 +242,8 @@ module pid(
Kd_d_1 <= Kd_d_0;
+ pid_motor_id_in_1 <= pid_motor_id_in_0;
+ pid_in_valid_1 <= pid_in_valid_0;
// pipeline stage 2: integrate err, calc err deriv
@@ -237,6 +261,9 @@ module pid(
Kp_d_2 <= Kp_d_1;
Ki_d_2 <= Ki_d_1;
Kd_d_2 <= Kd_d_1;
+
+ pid_motor_id_in_2 <= pid_motor_id_in_1;
+ pid_in_valid_2 <= pid_in_valid_1;
@@ -245,6 +272,8 @@ module pid(
// handle division/shifting by K_* denominator
err_3 <= err_2;
int_err_3 <= int_err_2;
+ pid_motor_id_in_3 <= pid_motor_id_in_2;
+ pid_in_valid_3 <= pid_in_valid_2;
case (Kp_d_2)
8'd0: pd_add_in_a <= kp_num_mult_out[12:0];
@@ -292,6 +321,8 @@ module pid(
int_err_4 <= int_err_3;
pid_add_in_a[12:0] <= pd_add_out[12:0];
pid_add_in_b[12:0] <= ki_contrib[12:0];
+ pid_motor_id_in_4 <= pid_motor_id_in_3;
+ pid_in_valid_4 <= pid_in_valid_3;
@@ -299,14 +330,17 @@ module pid(
err_r <= err_4;
int_err_r <= int_err_4;
pid_out <= pid_add_out;
+ pid_motor_id_in_5 <= pid_motor_id_in_4;
+ pid_in_valid_5 <= pid_in_valid_4;
end
// outputs
- assign pwm = pid_out[12:0];
+ assign pwm = (pid_out[12]) ? ~pid_out[11:0] : pid_out[11:0]; // drops a bit in negative case
assign err = err_r;
assign int_err = int_err_r;
- assign dir = !pid_out[12];
-
+ assign drive_code = {!pid_out[12],pid_out[12]};
+ assign pid_motor_id_out = pid_motor_id_in_5;
+ assign pid_out_valid = pid_in_valid_5;
endmodule
View
27 spi.v
@@ -48,7 +48,11 @@ module spi(
input [0:0] dig_update,
input [7:0] mot_drive_code,
input [4:0] mot_allstop,
- //input [0:0] bemf_calib_cmd,
+ input [15:0] pid_p_goal_0,
+ input [15:0] pid_p_goal_1,
+ input [15:0] pid_p_goal_2,
+ input [15:0] pid_p_goal_3,
+ input [3:0] pid_at_goal,
output reg [15:0] servo_pwm0_high_new,
output reg [15:0] servo_pwm1_high_new,
@@ -65,8 +69,14 @@ module spi(
output reg [0:0] dig_sample_new,
output reg [0:0] dig_update_new,
output reg [7:0] mot_drive_code_new,
- output reg [4:0] mot_allstop_new);
-// output reg [0:0] bemf_calib_cmd_new);
+ output reg [4:0] mot_allstop_new,
+ output reg [15:0] pid_p_goal_0_new,
+ output reg [15:0] pid_p_goal_1_new,
+ output reg [15:0] pid_p_goal_2_new,
+ output reg [15:0] pid_p_goal_3_new
+ );
+
+
@@ -161,7 +171,11 @@ module spi(
10'd38: SPI_OUT_tmp <= {15'd0, dig_update};//SPI_REGr[623:608];
10'd39: SPI_OUT_tmp <= {8'd0, mot_drive_code};//SPI_REGr[639:624];
10'd40: SPI_OUT_tmp <= {11'd0, mot_allstop}; //SPI_REGr[655:640];
- //10'd41: SPI_OUT_tmp <= {15'd0, bemf_calib_cmd};
+ 10'd41: SPI_OUT_tmp <= pid_p_goal_0;
+ 10'd42: SPI_OUT_tmp <= pid_p_goal_1;
+ 10'd43: SPI_OUT_tmp <= pid_p_goal_2;
+ 10'd44: SPI_OUT_tmp <= pid_p_goal_3;
+ 10'd45: SPI_OUT_tmp <= {12'd0, pid_at_goal};
default: SPI_OUT_tmp <= 16'd0;//SPI_REGr[15:0];
endcase
@@ -208,7 +222,10 @@ module spi(
dig_update_new <= (address == 10'd38) ? byte_data_received[0:0] : dig_update;
mot_drive_code_new <= (address == 10'd39) ? byte_data_received[7:0] : mot_drive_code;
mot_allstop_new <= (address == 10'd40) ? byte_data_received[4:0] : mot_allstop;
- //bemf_calib_cmd_new <= (address == 10'd41) ? byte_data_received[0:0] : bemf_calib_cmd;
+ pid_p_goal_0_new <= (address == 10'd41) ? byte_data_received[15:0] : pid_p_goal_0;
+ pid_p_goal_1_new <= (address == 10'd42) ? byte_data_received[15:0] : pid_p_goal_1;
+ pid_p_goal_2_new <= (address == 10'd43) ? byte_data_received[15:0] : pid_p_goal_2;
+ pid_p_goal_3_new <= (address == 10'd44) ? byte_data_received[15:0] : pid_p_goal_3;
end

0 comments on commit f88902b

Please sign in to comment.
Something went wrong with that request. Please try again.