tiny-ground-station/controlStepper.cpp at master · Key206/tiny-ground-station · GitHub

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
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include "controlStepper.h"

hw_timer_t *timer0 = NULL;
hw_timer_t *timer1 = NULL;

void IRAM_ATTR stopPWM1() {
  ledcWrite(PWM1_CHANNEL, 0);
  timerDetachInterrupt(timer0);
  timerEnd(timer0);
}
void IRAM_ATTR stopPWM2() {
  ledcWrite(PWM2_CHANNEL, 0);
  timerDetachInterrupt(timer1);
  timerEnd(timer1);
}
void setupPWM(){
  ledcAttachPin(STEP_AZ, PWM1_CHANNEL);
  ledcAttachPin(STEP_EL, PWM2_CHANNEL);
  ledcSetup(PWM1_CHANNEL, PWM_FREQ, PWM_RES);
  ledcSetup(PWM2_CHANNEL, PWM_FREQ, PWM_RES);
}
void rotateStepper(uint8_t typeMotor, double Angle){
  if(typeMotor){ // typeMotor == 1 => AZ stepper
    if(Angle > 0){
      digitalWrite(DIR_AZ, HIGH);
    }else{
      digitalWrite(DIR_AZ, LOW);
      Angle = -Angle;
    }
    timer0 = timerBegin(0, 80, true);
    timerAttachInterrupt(timer0, &stopPWM1, true);
    timerAlarmWrite(timer0, (uint32_t)TIMER_INTERVAL_US*Angle/360, true);
    ledcWrite(PWM1_CHANNEL, 50);
    timerAlarmEnable(timer0);
  }else{ // typeMotor == 0 => EL stepper
    if(Angle > 0){
      digitalWrite(DIR_EL, HIGH);
    }else{
      digitalWrite(DIR_EL, LOW);
      Angle = -Angle;
    }
    timer1 = timerBegin(1, 80, true);
    timerAttachInterrupt(timer1, &stopPWM2, true);
    timerAlarmWrite(timer1, (uint32_t)TIMER_INTERVAL_US*Angle/360, true);
    ledcWrite(PWM2_CHANNEL, 50);
    timerAlarmEnable(timer1);
  }
}
void rotateInTrackingMode(Sgp4& satInfo, bool resetFlag)
{
  static double preDegreeAz = 0;
  static double preDegreeEl = 0;

  double curDegreeAz = satInfo.satAz;
  double curDegreeEl = satInfo.satEl;

  double targetDegreeAz = curDegreeAz - preDegreeAz;
  double targetDegreeEl = curDegreeEl - preDegreeEl;

  if(targetDegreeAz < -CLOCKWISE_THRESHOLD){
    targetDegreeAz = (360 - preDegreeAz + curDegreeAz);
  }
  else if(targetDegreeAz > CLOCKWISE_THRESHOLD){
    targetDegreeAz = -(360 - curDegreeAz + preDegreeAz);
  }
  if(!resetFlag){
    if(targetDegreeAz >= MIN_DEGREE_ROTATE || targetDegreeAz <= -MIN_DEGREE_ROTATE){
      preDegreeAz = curDegreeAz;
      rotateStepper(STEPPER_AZ, targetDegreeAz);
    }
    if(targetDegreeEl >= MIN_DEGREE_ROTATE || targetDegreeEl <= -MIN_DEGREE_ROTATE){
        preDegreeEl = curDegreeEl;
        rotateStepper(STEPPER_EL, targetDegreeEl);
    }
  }else{
    rotateStepper(STEPPER_AZ, -preDegreeAz);
    rotateStepper(STEPPER_EL, -preDegreeEl);
    preDegreeAz = 0;
    preDegreeEl = 0;
    return;
  }
}
bool rotateInStandardMode(Sgp4& satInfo, unsigned long t_max)
{
  satInfo.findsat(t_max);

  double targetDegreeEl = satInfo.satEl;
  double targetDegreeAz = satInfo.satAz;

  rotateStepper(STEPPER_EL, targetDegreeEl);
  rotateStepper(STEPPER_AZ, targetDegreeAz);
  return true;
}