aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
blob: e4a5b362c33da8ae8f77b2e5298ed6a5c5dc56e1 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
pre { line-height: 125%; }
td.linenos .normal { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
span.linenos { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
td.linenos .special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
span.linenos.special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
.highlight .hll { background-color: #ffffcc }
.highlight .c { color: #888888 } /* Comment */
.highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */
.highlight .k { color: #008800; font-weight: bold } /* Keyword */
.highlight .ch { color: #888888 } /* Comment.Hashbang */
.highlight .cm { color: #888888 } /* Comment.Multiline */
.highlight .cp { color: #cc0000; font-weight: bold } /* Comment.Preproc */
.highlight .cpf { color: #888888 } /* Comment.PreprocFile */
.highlight .c1 { color: #888888 } /* Comment.Single */
.highlight .cs { color: #cc0000; font-weight: bold; background-color: #fff0f0 } /* Comment.Special */
.highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */
.highlight .ge { font-style: italic } /* Generic.Emph */
.highlight .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */
.highlight .gr { color: #aa0000 } /* Generic.Error */
.highlight .gh { color: #333333 } /* Generic.Heading */
.highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */
.highlight .go { color: #888888 } /* Generic.Output */
.highlight .gp { color: #555555 } /* Generic.Prompt */
.highlight .gs { font-weight: bold } /* Generic.Strong */
.highlight .gu { color: #666666 } /* Generic.Subheading */
.highlight .gt { color: #aa0000 } /* Generic.Traceback */
.highlight .kc { color: #008800; font-weight: bold } /* Keyword.C
/************************************************************************************************************************
 **************************************************************************************************************************/

#include <DataDef.h>
#include "include.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/Printing/JobSpool.pb-c.h"
#include "PMR/common/MessageContainer.pb-c.h"

#include "thread.h"
HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
HardwarePidControl MotorsControl[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};

bool BreakSensorenabled;
int32_t BreakSensordebouncetimemilli;

HardwarePidControlType ThreadMotorIdToControlId
">HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0}; //******************************************************************************************************************** uint32_t MotorsConfigMessage(HardwareMotor * request) { uint32_t status = PASSED; TimerMotors_t Motor_i; Motor_i = request->hardwaremotortype; /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) { if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype) { Motor_i = i; break; } } if (Motor_i == 0) { LOG_ERROR(request->hardwarepidcontroltype,"ERROR Control Id"); return ERROR; }*/ //if (Motor_i< MAX_THREAD_MOTORS_NUM) //{ memcpy (&MotorsCfg[Motor_i],request,sizeof(HardwareMotor)); status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]); // if (Motor_i == MOTOR_RDRIVING) // ThreadInitialTestStub(request); return status; // } // else return Motor_i; } 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; for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++) MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); temp=(10*(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*2); DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); return OK; } 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; BreakSensordebouncetimemilli = SensorCfg->debouncetimemilli; return OK; } return ERROR; } uint32_t thread_init(void) { //memset (MotorsCfg,0,sizeof(MotorsCfg)); //memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc)); return OK; }