diff --git a/src/comps/term.comp b/src/comps/term.comp index 5e9584d1..5383bd2f 100644 --- a/src/comps/term.comp +++ b/src/comps/term.comp @@ -36,6 +36,8 @@ HAL_PIN(send_step) = 10.0; HAL_PIN(buf_use) = 0.0; +HAL_PIN(fault) = 0.0; + #define TERM_BUF_SIZE 24 MEM(float w0[TERM_BUF_SIZE]); @@ -51,6 +53,8 @@ MEM(unsigned int read_pos) = 0; MEM(unsigned int write_pos) = 0; MEM(unsigned int send_counter) = 0; +MEM(unsigned int last_fault_sent) = 0; //save last error message + RT( if(send_counter++ >= PIN(send_step) - 1){ w0[write_pos] = PIN(wave0); @@ -121,6 +125,60 @@ NRT( } } + void fault_sender(){ + + int flt = (int)PIN(fault); + + if(flt != last_fault_sent){ + last_fault_sent = flt; + switch(flt){ + case STATE_DISABLED: + printf_("INFO: Disabled \n"); + break; + + case STATE_RESET: + printf_("INFO: Reset \n"); + break; + + case STATE_FB_ERROR: + printf_("ERROR: Feedback \n"); + break; + + case STATE_SAT_ERROR: + printf_("ERROR: Saturation \n"); + break; + + case STATE_OVR_CURR: + printf_("ERROR: Overcurrent \n"); + break; + + case STATE_POS_ERROR: + printf_("ERROR: Position \n"); + break; + + case STATE_OVR_TEMP: + printf_("ERROR: Overtemperture \n"); + break; + + case STATE_OVR_VOLT: + printf_("ERROR: Overvoltage \n"); + break; + + case STATE_ENABLED: + printf_("INFO: Enabled \n"); + break; + + case STATE_PHASING: + printf_("INFO: Phasing \n"); + break; + + default: + break; + } + } + } + + int e = 0; unsigned char buf[8 + 2]; @@ -144,6 +202,7 @@ NRT( float g6 = PIN(gain6); float g7 = PIN(gain7); + unsigned int wp = write_pos; unsigned int bc = 0; @@ -185,11 +244,15 @@ NRT( PIN(buf_use) = bc; + + if(UB_USB_CDC_GetStatus()==USB_CDC_CONNECTED && systime >= 1000){ char source[APP_TX_BUF_SIZE]; char sink[APP_TX_BUF_SIZE]; + fault_sender(); + int i = -1; char rx_buf[APP_TX_BUF_SIZE]; if(UB_USB_CDC_ReceiveString(rx_buf)==RX_READY){ diff --git a/src/defines_res.h b/src/defines_res.h index b4403b5a..08f8705d 100644 --- a/src/defines_res.h +++ b/src/defines_res.h @@ -87,6 +87,20 @@ #define FAN_PORT GPIOC #define FAN_IO_RCC RCC_AHB1Periph_GPIOC +//states + +// state -10 = reset state, -6 = fb error, -5 = sat error, -4 = over current, -3 = pos_error, -2 = over temperature, -1 = over voltage, 0 = disabled, 1 = enabled, 2 = phasing +#define STATE_RESET -10 +#define STATE_FB_ERROR -6 +#define STATE_SAT_ERROR -5 +#define STATE_OVR_CURR -4 +#define STATE_POS_ERROR -3 +#define STATE_OVR_TEMP -2 +#define STATE_OVR_VOLT -1 +#define STATE_DISABLED 0 +#define STATE_ENABLED 1 +#define STATE_PHASING 2 + #ifdef TROLLER #define UART_DRV USART1 diff --git a/src/link.c b/src/link.c index 75983fd9..032fd590 100644 --- a/src/link.c +++ b/src/link.c @@ -101,6 +101,7 @@ void link_pid(){ link_hal_pins("fault0.led_green", "led0.g"); link_hal_pins("fault0.led_red", "led0.r"); + link_hal_pins("fault0.state", "term0.fault"); //sent fault to terminal // @@ -844,7 +845,7 @@ void set_8MSA3M(){ //B&R 8MSA3M } void set_gas(){ - set_hal_pin("conf0.polecount", 3.0); + set_hal_pin("conf0.polecount", 2.0); set_hal_pin("conf0.max_vel", RPM(9000)); set_hal_pin("conf0.max_acc", RPM(9000) / 0.05); set_hal_pin("conf0.max_force", 98); //Datasheet @@ -852,19 +853,18 @@ void set_gas(){ set_hal_pin("conf0.fb_type", SINCOS1); set_hal_pin("conf0.fb_rev", 1.0); - set_hal_pin("conf0.fb_res", 2048.0); + set_hal_pin("conf0.fb_res", 1.0); set_hal_pin("conf0.r", 7); //mess set_hal_pin("conf0.l", 0.002);//Datasheet set_hal_pin("conf0.j", 0.000020);//ToDo set_hal_pin("conf0.psi", 0.29 / 2.0 / M_PI);//ToDo - set_hal_pin("conf0.pos_p", 100.0);//ToDo - set_hal_pin("conf0.acc_p", 0.2);//ToDo + set_hal_pin("conf0.pos_p", 30.0);//ToDo + set_hal_pin("conf0.acc_p", 0.1);//ToDo set_hal_pin("conf0.acc_pi", 50.0);//ToDo - set_hal_pin("conf0.cur_p", 0.5);//ToDo + set_hal_pin("conf0.cur_p", 0.4);//ToDo set_hal_pin("conf0.cur_i", 0.001);//ToDo set_hal_pin("conf0.cur_ff", 1.0);//ToDo - set_hal_pin("enc_fb0.res", 512.0);//ToDo set_fb_sincos(); } diff --git a/term/dqdraw.cpp b/term/dqdraw.cpp index 96669cc0..a82cd080 100644 --- a/term/dqdraw.cpp +++ b/term/dqdraw.cpp @@ -4,12 +4,12 @@ dqDraw::dqDraw(wxFrame* parent) : wxPanel(parent){ Bind(wxEVT_PAINT, &dqDraw::paintEvent, this); Bind(wxEVT_MOTION, &dqDraw::mouseEvent, this); Bind(wxEVT_MOUSEWHEEL , &dqDraw::scrollEvent, this); - + scale = 5; iscale = 1; In = 10; Un = 20; - + // Rs = 15;//7.6; // Psi = 0.37;//0.5411; // P = 3; @@ -35,7 +35,7 @@ void dqDraw::Update() { Uabs = sqrt(Ud*Ud + Uq*Uq); Iabs = sqrt(Id*Id + Iq*Iq); - + // Permanentmagnet-, Reluktanz- und Gesamtmoment // M_perm = 1.5f * P * Psi * Iq; @@ -51,12 +51,12 @@ void dqDraw::SetCurrent(double d, double q) double OmegaPsi = Omega * Psi; double Xd = Omega * Ld; double Xq = Omega * Lq; - + Id = d; Iq = q; Ud = -Iq * Xq + Id * Rs; Uq = OmegaPsi + Id * Xd + Iq * Rs; - + Update(); } @@ -65,12 +65,12 @@ void dqDraw::SetVoltage(double d, double q) double OmegaPsi = Omega * Psi; double Xd = Omega * Ld; double Xq = Omega * Lq; - + Ud = d; Uq = q; Id = (Rs * d + Xq * (q - OmegaPsi)) / (Rs * Rs + Xd * Xq); Iq = (Rs * (q - OmegaPsi) - Xd * d) / (Rs * Rs + Xd * Xq); - + Update(); } @@ -100,13 +100,13 @@ void dqDraw::paintEvent(wxPaintEvent & evt) void dqDraw::render(wxDC& dc) { dc.GetSize(&w, &h); - + dc.SetPen(*wxGREY_PEN); dc.DrawLine( 0, h/2, w, h/2 ); dc.DrawLine( w/2, 0, w/2, h ); - + // ursprung oben links - + dc.SetBrush(*wxTRANSPARENT_BRUSH); dc.SetPen(*wxRED_PEN); dc.DrawLine( w/2, h/2, Ud*scale+w/2, h/2-Uq*scale); @@ -114,4 +114,4 @@ void dqDraw::render(wxDC& dc) dc.SetPen(*wxBLUE_PEN); dc.DrawLine( w/2, h/2, Id*scale*iscale+w/2, h/2-Iq*scale*iscale); dc.DrawCircle(w/2, h/2, In*scale*iscale); -} \ No newline at end of file +} diff --git a/term/main.cpp b/term/main.cpp index 8e5d28c7..8ab11501 100644 --- a/term/main.cpp +++ b/term/main.cpp @@ -43,7 +43,7 @@ bool Servoterm::OnInit() writeID = wxNewId(); readID = wxNewId(); dqID = wxNewId(); - + menuBar->Append(fileMenu, "&File"); fileMenu->Append(wxID_OPEN); Bind(wxEVT_COMMAND_MENU_SELECTED, &Servoterm::OnOpen, this, wxID_OPEN); @@ -53,11 +53,11 @@ bool Servoterm::OnInit() Bind(wxEVT_COMMAND_MENU_SELECTED, &Servoterm::OnWrite, this, writeID); fileMenu->Append(readID, "&Read"); Bind(wxEVT_COMMAND_MENU_SELECTED, &Servoterm::OnRead, this, readID); - + menuBar->Append(viewMenu, "&View"); viewMenu->Append(dqID, "&DQsim"); Bind(wxEVT_COMMAND_MENU_SELECTED, &Servoterm::OnDq, this, dqID); - + wxMenu *helpMenu = new wxMenu; menuBar->Append(helpMenu, "&Help" ); helpMenu->Append(wxID_ABOUT); @@ -67,7 +67,7 @@ bool Servoterm::OnInit() //frame->CreateStatusBar(); //frame->SetStatusText("Statuskram und so"); - + servoframe->Show(TRUE); return TRUE; } @@ -93,7 +93,7 @@ void Servoterm::OnWrite(wxCommandEvent& WXUNUSED(event)){ } void Servoterm::OnSave(wxCommandEvent& WXUNUSED(event)){ - + } void Servoterm::OnOpen(wxCommandEvent& WXUNUSED(event)){ @@ -117,4 +117,4 @@ void Servoterm::OnAbout(wxCommandEvent& WXUNUSED(event)){ aboutInfo.AddDeveloper("Rene-dev"); wxAboutBox(aboutInfo); */ -} \ No newline at end of file +} diff --git a/term/mainframe.cpp b/term/mainframe.cpp index cfde2b53..b32e4f74 100644 --- a/term/mainframe.cpp +++ b/term/mainframe.cpp @@ -32,6 +32,8 @@ ServoFrame::ServoFrame(const wxString& title) : wxFrame(NULL, wxID_ANY, title){ connectbutton = new wxButton(top, wxID_ANY, wxT("&Connect")); clear = new wxButton(top, wxID_ANY, wxT("Clear")); refresh = new wxButton(top, wxID_ANY, wxT("&Refresh")); + reset = new wxButton(top, wxID_ANY, wxT("Reset Fault")); + reset->Disable(); uhu = new wxRadioButton(top,wxID_ANY, "UHU"); stmbl = new wxRadioButton(top, wxID_ANY,"STMBL"); stmbl->SetValue(true); @@ -39,11 +41,13 @@ ServoFrame::ServoFrame(const wxString& title) : wxFrame(NULL, wxID_ANY, title){ refresh->Bind(wxEVT_BUTTON, &ServoFrame::OnRefresh, this, wxID_ANY); connectbutton->Bind(wxEVT_BUTTON, &ServoFrame::OnConnect, this, wxID_ANY); clear->Bind(wxEVT_BUTTON, &ServoFrame::OnClear, this, wxID_ANY); + reset->Bind(wxEVT_BUTTON, &ServoFrame::OnReset, this, wxID_ANY); listports(); leiste->Add(choose_port, 0,wxALIGN_LEFT|wxALL,3); leiste->Add(connectbutton,0,wxALIGN_LEFT|wxALL,3); leiste->Add(refresh,0,wxALIGN_LEFT|wxALL,3); leiste->Add(clear,0,wxALIGN_LEFT|wxALL,3); + leiste->Add(reset,0,wxALIGN_LEFT|wxALL,3); leiste->Add(uhu,0,wxALIGN_LEFT|wxALL,3); leiste->Add(stmbl,0,wxALIGN_LEFT|wxALL,3); topsizer->Add(leiste); @@ -135,7 +139,7 @@ void ServoFrame::OnKeyDown(wxKeyEvent& event){ } void ServoFrame::OnColorChange(wxMouseEvent& event){ - + } void ServoFrame::OnChannelChange(wxCommandEvent& event){ @@ -201,7 +205,7 @@ void ServoFrame::OnTimer(wxTimerEvent& evt){ text->AppendText(wxString::FromAscii(buf[i] & 0x7f)); } } - + }else if(stmbl->GetValue()){ for (int i=0; i= 0){ @@ -225,6 +229,28 @@ void ServoFrame::OnRefresh(wxCommandEvent& WXUNUSED(event)){ listports(); } +void ServoFrame::OnReset(wxCommandEvent& WXUNUSED(event)){ + string s1 = "fault0.reset = 1"; + string s2 = "fault0.reset = 0"; + if(connected){ + int ret1 = sp_nonblocking_write(port, s1.c_str(), s1.length()); + int ret2 = sp_nonblocking_write(port, "\r", 1); + int ret3 = sp_nonblocking_write(port, s2.c_str(), s2.length()); + int ret4 = sp_nonblocking_write(port, "\r", 1); + + + if(ret1 != s1.length() || ret2!=1 || ret3 != s2.length() || ret4!=1){ + wxMessageBox( wxT("Fehler beim senden"), wxT("Error"), wxICON_EXCLAMATION); + disconnect(); + return; + } + }else{ + wxMessageBox( wxT("Nicht verbunden"), wxT("Error"), wxICON_EXCLAMATION); + return; + } + return; +} + void ServoFrame::listports(){ if (!ports) { sp_free_port_list(ports); @@ -271,6 +297,7 @@ void ServoFrame::connect(){ connected = true; connectbutton->SetLabel(wxT("&Disonnect")); refresh->Disable(); + reset->Enable(); choose_port->Disable(); uhu->Disable(); stmbl->Disable(); @@ -287,6 +314,7 @@ void ServoFrame::disconnect(){ connected = false; connectbutton->SetLabel(wxT("&Connect")); refresh->Enable(); + reset->Disable(); choose_port->Enable(); uhu->Enable(); stmbl->Enable(); diff --git a/term/mainframe.hpp b/term/mainframe.hpp index 95047c79..5b93a33b 100644 --- a/term/mainframe.hpp +++ b/term/mainframe.hpp @@ -26,6 +26,7 @@ private: wxButton *connectbutton; wxButton *refresh; wxButton *clear; + wxButton *reset; wxRadioButton *uhu; wxRadioButton *stmbl; wxTimer * timer; @@ -39,6 +40,7 @@ private: bool connected; void OnConnect(wxCommandEvent& WXUNUSED(event)); void OnRefresh(wxCommandEvent& WXUNUSED(event)); + void OnReset(wxCommandEvent& WXUNUSED(event)); void OnClear(wxCommandEvent& WXUNUSED(event)); void OnInput(wxCommandEvent& event); void OnColorChange(wxMouseEvent& event);