using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media.Imaging;
namespace Tango.Scripting.IDE.ProjectItems
{
public class ReferenceAssembliesItem : ProjectItem
{
p/************************************************************************************************************************
**************************************************************************************************************************/
#include <DataDef.h>
#include <Drivers/FPGA/Motors_Driver/PowerSTEP01.h>
#include "include.h"
#include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
#include "PMR/Hardware/HardWareConfiguration.pb-c.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareDancer.pb-c.h"
#include "PMR/Hardware/HardwareWinder.pb-c.h"
#include "PMR/Hardware/HardwareBreakSensor.pb-c.h"
#include "PMR/Diagnostics/EventType.pb-c.h"
#include "Modules/AlarmHandling/AlarmHandling.h"
#include "PMR/Printing/JobSpool.pb-c.h"
#include "PMR/common/MessageContainer.pb-c.h"
#include "Common/SWUpdate/FileSystem.h"
#include "drivers/Flash_Memory/fatfs/ff.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"
#include "drivers/Flash_ram/FlashProgram.h"
#include "drivers/Flash_ram/MCU_E2Prom.h"
#include "modules/ids/ids_ex.h"
#include "thread.h"
MotorDriverConfigStruc MotorsCfg[NUM_OF_MOTORS]={0};
HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
#define BREAK_SENSOR_LIMIT 10
bool BreakSensorenabled;
int32_t BreakSensordebouncetimemilli = BREAK_SENSOR_LIMIT;
HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};
//********************************************************************************************************************
uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
{
uint32_t status = PASSED;
TimerMotors_t Motor_i,MotorId;
HardwareMotor *request;
if (HWrequest == NULL)
return ERROR;
AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,OFF);
for (MotorId = 0; MotorId < HWrequest->n_motors ; MotorId++)
{
request = HWrequest->motors[MotorId];
if (request == NULL)
return ERROR;
Motor_i = request->hardwaremotortype;
MotorsCfg[Motor_i].configword = request->configword;
MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype;
MotorsCfg[Motor_i].minfrequency = request->minfrequency;
MotorsCfg[Motor_i].maxfrequency = request->maxfrequency;
MotorsCfg[Motor_i].setmicrostep = request->setmicrostep;
MotorsCfg[Motor_i].microstep = request->microstep;
MotorsCfg[Motor_i].maxchangeslope = request->maxchangeslope;
MotorsCfg[Motor_i].highlengthmicrosecond = request->highlengthmicrosecond;
MotorsCfg[Motor_i].speedmaster = request->speedmaster;
MotorsCfg[Motor_i].pulseperround = request->pulseperround;
MotorsCfg[Motor_i].pulleyradius = request->pulleyradius;
MotorsCfg[Motor_i].configword = request->configword;
if(MotorDriverResponse[Motor_i].DriverType == CombinrdMotDriver)
{
MotorsCfg[Motor_i].directionthreadwize = request->directionthreadwize ^ 1;//PowerSTEP01
}
else
{
MotorsCfg[Motor_i].directionthreadwize = request->directionthreadwize;//L6470 + L6472
}
MotorsCfg[Motor_i].kvalhold = request->kvalhold;
MotorsCfg[Motor_i].kvalrun = request->kvalrun;
MotorsCfg[Motor_i].kvalacc = request->kvalacc;
MotorsCfg[Motor_i].kvaldec = request->kvaldec;
MotorsCfg[Motor_i].overcurrentthreshold = request->overcurrentthreshold;
MotorsCfg[Motor_i].stallthreshold = request->stallthreshold;
MotorsCfg[Motor_i].thermalcompensationfactor = request->thermalcompensationfactor;
MotorsCfg[Motor_i].lowspeedoptimization = request->lowspeedoptimization;
MotorsCfg[Motor_i].stslp = request->stslp;
MotorsCfg[Motor_i].intspd = request->intspd;
MotorsCfg[Motor_i].fnslpacc = request->fnslpacc;
MotorsCfg[Motor_i].fnslpdec = request->fnslpdec;
MotorsCfg[Motor_i].fsspd = request->fsspd;
MotorsCfg[Motor_i].gatecfg1 = request->gatecfg1;
MotorsCfg[Motor_i].gatecfg2 = request->gatecfg2;
MotorsCfg[Motor_i].tvalhold = request->tvalhold;
MotorsCfg[Motor_i].tvalrun = request->tvalrun ;
MotorsCfg[Motor_i].tvalacc = request->tvalacc ;
MotorsCfg[Motor_i].tvaldec = request->tvaldec ;
MotorsCfg[Motor_i].tfast = request->tfast ;
MotorsCfg[Motor_i].tonmin = request->tonmin ;
MotorsCfg[Motor_i].toffmin = request->toffmin ;
status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]);
if ((Motor_i >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_i <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8))
{
IDS_Dispenser_Init(Motor_i-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
}
// if (Motor_i == MOTOR_RDRIVING)
// ThreadInitialTestStub(request);
}
return status;
}
uint32_t MotorPidRequestMessage(HardwarePidControl* request)
{
int Motor_i,i;
int temp;
for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
{
if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
{
Motor_i = i;
break;
}
}
memcpy (&MotorsControl[Motor_i],request,sizeof(HardwarePidControl));
if (MotorsControl[Motor_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
MotorsControl[Motor_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
#ifdef TEST_LONGER_PID_THREAD
MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
#endif
for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
{
MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning
MotorSpeedSamples[Motor_i][i] = 0;
}
NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
// uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
return OK;
}
char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
uint32_t StoreDancerConfigMessage()
{
uint32_t status = OK;
// FRESULT Fresult = FR_OK;
HardwareConfiguration DancerConfig;
HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
// uint8_t* response_buffer;
// size_t response_size = 0;
int Dancer_i;
hardware_configuration__init(&DancerConfig);
DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
{
hardware_dancer__init(&Dancers[Dancer_i]);
DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
Dancers[Dancer_i].has_zeropoint = true;
Dancers[Dancer_i].hardwaredancertype = Dancer_i;
Dancers[Dancer_i].has_hardwaredancertype = true;
DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0,0);
DancerConfig.n_dancers++;
DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
}
status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
/*
response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
if (response_buffer)
{
response_size = hardware_configuration__pack(&DancerConfig, response_buffer);
}
Fresult = FileWrite(response_buffer,response_size,DancerConfigPath,BIOS_WAIT_FOREVER);
EraseFlashSection(DANCERS_MAP_IN_FLASH,1024);
for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
{
ReadAppAndProgram(DANCERS_MAP_IN_FLASH, sizeof(Dancers), Dancers);
}
my_free(response_buffer);
*/
return status;
}
uint32_t LoadDancerConfigMessage()
{
uint32_t status = OK;
MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint);
MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint);
MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint);
status |= Report("Dancer 0 Store data eeprom ",__FILE__,__LINE__,0,RpWarning,(int)DancersCfg[0].zeropoint,0);
status |= Report("Dancer 1 Store data eeprom ",__FILE__,__LINE__,1,RpWarning,(int)DancersCfg[1].zeropoint,0);
status |= Report("Dancer 2 Store data eeprom ",__FILE__,__LINE__,2,RpWarning,(int)DancersCfg[2].zeropoint,0);
return status;
}
uint32_t DancerConfigMessage(HardwareDancer * request)
{
uint32_t status = PASSED;
int Dancer_i;
Dancer_i = request->hardwaredancertype;
if (Dancer_i<MAX_SYSTEM_DANCERS )
{
memcpy (&DancersCfg[Dancer_i],request,sizeof(HardwareDancer));
return status;
}
else
return Dancer_i;
}
uint32_t ThreadConfigBreakSensor(void *request)
{
HardwareBreakSensor *SensorCfg = (HardwareBreakSensor*)request;
if (SensorCfg)
{
BreakSensorenabled = SensorCfg->enabled;
if (SensorCfg->debouncetimemilli)
{
BreakSensordebouncetimemilli = SensorCfg->debouncetimemilli;
}
return OK;
}
return ERROR;
}
uint32_t thread_init(void)
{
//memset (MotorsCfg,0,sizeof(MotorsCfg));
//memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc));
return OK;
}
void ThreadSetBreakSensorLimit(int limit)
{
if (limit)
BreakSensordebouncetimemilli = limit;
}
