Exemplo n.º 1
0
int main(void)
{
   // Relocate interrupt vectors
   //
   extern void *g_pfnVectors;
   SCB->VTOR = (uint32_t)&g_pfnVectors;

   float period = 0.0;
   int last_start = 0;
   int start = 0;
   int end = 0;

   setup();
   init_hal();

   set_comp_type("foo"); // default pin for mem errors
   HAL_PIN(bar) = 0.0;

   //feedback comps
   #include "comps/adc.comp"
   #include "comps/res.comp"
   #include "comps/enc_fb.comp"
   #include "comps/encm.comp"
   #include "comps/encs.comp"
   #include "comps/yaskawa.comp"
   //TODO: hyperface

   //command comps
   #include "comps/sserial.comp"
   #include "comps/sim.comp"
   #include "comps/enc_cmd.comp"
   #include "comps/en.comp"

   //PID
   #include "comps/stp.comp"
   #include "comps/rev.comp"
   #include "comps/rev.comp"
   #include "comps/vel.comp"
   #include "comps/vel.comp"
   #include "comps/cauto.comp"
   #include "comps/pid.comp"
   #include "comps/pmsm_t2c.comp"
   #include "comps/curpid.comp"
   #include "comps/pmsm.comp"
   #include "comps/pmsm_limits.comp"
   #include "comps/idq.comp"
   #include "comps/dq.comp"
   #include "comps/hv.comp"

   //other comps
   #include "comps/fault.comp"
   #include "comps/term.comp"
   #include "comps/io.comp"


   set_comp_type("net");
   HAL_PIN(enable) = 0.0;
   HAL_PIN(cmd) = 0.0;
   HAL_PIN(fb) = 0.0;
   HAL_PIN(fb_error) = 0.0;
   HAL_PIN(cmd_d) = 0.0;
   HAL_PIN(fb_d) = 0.0;
   HAL_PIN(core_temp0) = 0.0;
   HAL_PIN(core_temp1) = 0.0;
   HAL_PIN(motor_temp) = 0.0;
   HAL_PIN(rt_calc_time) = 0.0;
   HAL_PIN(frt_calc_time) = 0.0;
   HAL_PIN(nrt_calc_time) = 0.0;
   HAL_PIN(rt_period) = 0.0;
   HAL_PIN(frt_period) = 0.0;
   HAL_PIN(nrt_period) = 0.0;

   set_comp_type("conf");
   HAL_PIN(r) = 1.0;
   HAL_PIN(l) = 0.01;
   HAL_PIN(j) = KGCM2(0.26);
   HAL_PIN(psi) = 0.05;
   HAL_PIN(polecount) = 4.0;
   HAL_PIN(mot_type) = 0.0;//ac sync,async/dc,2phase
   HAL_PIN(out_rev) = 0.0;
   HAL_PIN(high_motor_temp) = 80.0;
   HAL_PIN(max_motor_temp) = 100.0;
   HAL_PIN(phase_time) = 0.5;
   HAL_PIN(phase_cur) = 1.0;

   HAL_PIN(max_vel) = RPM(1000.0);
   HAL_PIN(max_acc) = RPM(1000.0)/0.01;
   HAL_PIN(max_force) = 1.0;
   HAL_PIN(max_dc_cur) = 1.0;
   HAL_PIN(max_ac_cur) = 2.0;

   HAL_PIN(fb_type) = RES;
   HAL_PIN(fb_polecount) = 1.0;
   HAL_PIN(fb_offset) = 0.0;
   HAL_PIN(fb_rev) = 0.0;
   HAL_PIN(fb_res) = 1000.0;
   HAL_PIN(autophase) = 1.0;//constant,cauto,hfi

   HAL_PIN(cmd_type) = ENC;
   HAL_PIN(cmd_unit) = 0.0;//pos,vel,torque
   HAL_PIN(cmd_rev) = 0.0;
   HAL_PIN(cmd_res) = 2000.0;
   HAL_PIN(en_condition) = 0.0;
   HAL_PIN(error_out) = 0.0;
   HAL_PIN(pos_static) = 0.0;//track pos in disabled and error condition

   HAL_PIN(sin_offset) = 0.0;
   HAL_PIN(cos_offset) = 0.0;
   HAL_PIN(sin_gain) = 1.0;
   HAL_PIN(cos_gain) = 1.0;
   HAL_PIN(max_dc_volt) = 370.0;
   HAL_PIN(max_hv_temp) = 90.0;
   HAL_PIN(max_core_temp) = 55.0;
   HAL_PIN(max_pos_error) = M_PI / 2.0;
   HAL_PIN(high_dc_volt) = 350.0;
   HAL_PIN(low_dc_volt) = 12.0;
   HAL_PIN(high_hv_temp) = 70.0;
   HAL_PIN(fan_hv_temp) = 60.0;
   HAL_PIN(fan_core_temp) = 450.0;
   HAL_PIN(fan_motor_temp) = 60.0;

   HAL_PIN(p) = 0.99;
   HAL_PIN(pos_p) = 100.0;
   HAL_PIN(vel_p) = 1.0;
   HAL_PIN(acc_p) = 0.3;
   HAL_PIN(acc_pi) = 50.0;
   HAL_PIN(cur_p) = 0.0;
   HAL_PIN(cur_i) = 0.0;
   HAL_PIN(cur_ff) = 1.0;
   HAL_PIN(cur_ind) = 0.0;
   HAL_PIN(max_sat) = 0.2;

   rt_time_hal_pin = map_hal_pin("net0.rt_calc_time");
   frt_time_hal_pin = map_hal_pin("net0.frt_calc_time");
   rt_period_time_hal_pin = map_hal_pin("net0.rt_period");
   frt_period_time_hal_pin = map_hal_pin("net0.frt_period");

   for(int i = 0; i < hal.nrt_init_func_count; i++){ // run nrt init
      hal.nrt_init[i]();
   }

   link_pid();


   if(hal.pin_errors + hal.comp_errors == 0){
      start_hal();
   }
   else{
      hal.hal_state = MEM_ERROR;
   }

   while(1)//run non realtime stuff
   {
      start = SysTick->VAL;

      if(last_start < start){
        last_start += SysTick->LOAD;
      }

      period = ((float)(last_start - start)) / RCC_Clocks.HCLK_Frequency;
      last_start = start;

      for(hal.active_nrt_func = 0; hal.active_nrt_func < hal.nrt_func_count; hal.active_nrt_func++){//run all non realtime hal functions
         hal.nrt[hal.active_nrt_func](period);
      }
      hal.active_nrt_func = -1;

      end = SysTick->VAL;
      if(start < end){
        start += SysTick->LOAD;
      }
      PIN(nrt_calc_time) = ((float)(start - end)) / RCC_Clocks.HCLK_Frequency;
      PIN(nrt_period) = period;
      Wait(2);
   }
}
Exemplo n.º 2
0
int main(void)
{
	float period = 0.0;
	float lasttime = 0.0;

	setup();

	#include "comps/adc.comp"
  #include "comps/fault.comp"
	#include "comps/enc_cmd.comp"
  #include "comps/enc_fb.comp"
	//#include "comps/res.comp"
	#include "comps/encm.comp"
	#include "comps/sim.comp"

	#include "comps/rev.comp"
	#include "comps/rev.comp"

	#include "comps/cauto.comp"

	#include "comps/pderiv.comp"
	#include "comps/pderiv.comp"

	#include "comps/pid.comp"

	#include "comps/rev.comp"

	#include "comps/cur.comp"

	#include "comps/pwm2uart.comp"

	#include "comps/absavg.comp"

	#include "comps/term.comp"
	#include "comps/led.comp"
	#include "comps/fan.comp"
	#include "comps/brake.comp"


	set_comp_type("net");
	HAL_PIN(enable) = 0.0;
	HAL_PIN(cmd) = 0.0;
	HAL_PIN(fb) = 0.0;
  HAL_PIN(fb_error) = 0.0;
	HAL_PIN(cmd_d) = 0.0;
	HAL_PIN(fb_d) = 0.0;
	HAL_PIN(amp) = 0.0;
	HAL_PIN(vlt) = 0.0;
	HAL_PIN(tmp) = 0.0;
	HAL_PIN(rt_calc_time) = 0.0;

	set_comp_type("conf");
	HAL_PIN(r) = 0.0;
	HAL_PIN(l) = 0.0;
	HAL_PIN(j) = 0.0;
	HAL_PIN(km) = 0.0;
	HAL_PIN(pole_count) = 0.0;
	HAL_PIN(fb_pole_count) = 0.0;
	HAL_PIN(fb_offset) = 0.0;
	HAL_PIN(pos_p) = 0.0;
	HAL_PIN(acc_p) = 0.0;
	HAL_PIN(acc_pi) = 0.0;
	HAL_PIN(cur_lp) = 0.0;
	HAL_PIN(max_vel) = 0.0;
	HAL_PIN(max_acc) = 0.0;
	HAL_PIN(max_force) = 0.0;
	HAL_PIN(max_cur) = 0.0;
	HAL_PIN(fb_type) = 0.0;
	HAL_PIN(cmd_type) = 0.0;
	HAL_PIN(fb_rev) = 0.0;
	HAL_PIN(cmd_rev) = 0.0;
	HAL_PIN(out_rev) = 0.0;
	HAL_PIN(fb_res) = 1.0;
	HAL_PIN(cmd_res) = 2000.0;
	HAL_PIN(sin_offset) = -17600.0;
	HAL_PIN(cos_offset) = -17661.0;
	HAL_PIN(sin_gain) = 0.0001515;
	HAL_PIN(cos_gain) = 0.00015;

  HAL_PIN(max_volt) = 370.0;
  HAL_PIN(max_temp) = 100.0;
  HAL_PIN(max_pos_error) = M_PI / 2.0;
  HAL_PIN(high_volt) = 350.0;
  HAL_PIN(low_volt) = 12.0;
  HAL_PIN(high_temp) = 80.0;
  HAL_PIN(fan_temp) = 40.0;
  HAL_PIN(autophase) = 1.0;
  HAL_PIN(max_sat) = 0.2;

	g_amp_hal_pin = map_hal_pin("net0.amp");
	g_vlt_hal_pin = map_hal_pin("net0.vlt");
	g_tmp_hal_pin = map_hal_pin("net0.tmp");
	rt_time_hal_pin = map_hal_pin("net0.rt_calc_time");

	for(int i = 0; i < hal.init_func_count; i++){
		hal.init[i]();
	}

	link_pid();

	//set_e240();
	//set_bergerlahr();//pid2: ok
	//set_mitsubishi();//pid2: ok
	//set_festo();
	//set_manutec();
	set_rexroth();//pid2: ok
  //link_hal_pins("enc10.ipos", "rev1.in");

	//set_hal_pin("res0.reverse", 0.0);
	//set_bosch1();//pid2: ok
	//set_bosch4();//pid2: ok
	//set_sanyo();//pid2: ok
	//set_br();

  set_cmd_enc();



  link_hal_pins("conf0.max_cur", "fault0.max_cur");
  link_hal_pins("conf0.max_volt", "fault0.max_volt");
  link_hal_pins("conf0.max_temp", "fault0.max_temp");
  link_hal_pins("conf0.max_pos_error", "fault0.max_pos_error");
  link_hal_pins("conf0.high_volt", "fault0.high_volt");
  link_hal_pins("conf0.high_temp", "fault0.high_temp");
  link_hal_pins("conf0.low_volt", "fault0.low_volt");
  link_hal_pins("conf0.fan_temp", "fault0.fan_temp");
  link_hal_pins("conf0.autophase", "fault0.phase_on_start");
  link_hal_pins("conf0.max_sat", "fault0.max_sat");

  set_hal_pin("fault0.reset", 0.0);

  link_hal_pins("fault0.phase_start", "cauto0.start");
  link_hal_pins("cauto0.ready", "fault0.phase_ready");

  link_hal_pins("pid0.pos_error", "fault0.pos_error");
  link_hal_pins("pid0.saturated", "fault0.sat");
  link_hal_pins("net0.vlt", "fault0.volt");
  link_hal_pins("net0.tmp", "fault0.temp");
  link_hal_pins("net0.amp", "fault0.amp");
  link_hal_pins("net0.fb_error", "fault0.fb_error");
  link_hal_pins("net0.cmd", "fault0.cmd");
  link_hal_pins("rev1.out", "fault0.fb");
  link_hal_pins("fault0.start_offset", "cauto0.start_offset");

  link_hal_pins("fault0.cur", "pid0.max_cur");
  link_hal_pins("fault0.cur", "cur0.cur_max");

  link_hal_pins("fault0.brake", "brake0.brake");
  link_hal_pins("fault0.fan", "fan0.fan");

  link_hal_pins("fault0.enable_out", "pwm2uart0.enable");
  link_hal_pins("fault0.enable_pid", "pid0.enable");

  link_hal_pins("net0.enable", "fault0.enable");

  link_hal_pins("fault0.led_green", "led0.g");
  link_hal_pins("fault0.led_red", "led0.r");


	link_hal_pins("pid0.pos_error", "avg0.in");
	set_hal_pin("avg0.ac", 0.0001);




	TIM_Cmd(TIM8, ENABLE);//int

	UB_USB_CDC_Init();

	while(1)  // Do not exit
	{
		Wait(2);
		period = systime/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0 - lasttime;
		lasttime = systime/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0;
		for(int i = 0; i < hal.nrt_func_count; i++){
			hal.nrt[i](period);
		}
	}
}