main.cpp 8.03 KB
Newer Older
Jake Read's avatar
Jake Read committed
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
#include <Arduino.h>

#include "drivers/indicators.h"
#include "utils/cobs.h"
#include "osap/osap.h"

OSAP* osap = new OSAP("cl stepper motor drop");

#include "osap/vport_usbserial.h"
VPort_USBSerial* vPortSerial = new VPort_USBSerial();
#include "drivers/ucbus_drop.h"
#include "drivers/step_cl.h"

#include "utils/clocks_d51_module.h"

union chunk_float32 {
  uint8_t bytes[4];
  float f;
};

union chunk_float64 {
  uint8_t bytes[8];
  double f;
};

// adhoc reply 
uint8_t reply[1024];
uint16_t rl = 0;

uint8_t replyBlankPck[1024];
uint16_t replyBlankPl = 0;
uint16_t replyBlankPtr = 0;
uint16_t replyBlankSegsize = 0;
VPort* replyBlankVp;
uint16_t replyBlankVpi = 0;
uint16_t lastQueueSpaceTxd = 0;

#define BUS_DROP 1    // Z: 1, YL: 2, X: 3, YR: 4
#define AXIS_PICK 2   // Z: 2, Y: 1, X: 0
#define AXIS_INVERT false  // Z: false, YL: true, YR: false, X: false
#define SPU 3200.0F // always positive! Z: 3200, XY: 400 
#define C_SCALE 0.2F // 0-1, floating: initial holding current to motor, 0-2.5A 
#define TICKS_PER_PACKET 20.0F // always 20.0F

void setup() {
  ERRLIGHT_SETUP;
  CLKLIGHT_SETUP;
  DEBUG1PIN_SETUP;
  // osap
  osap->description = "cl controller test";
  // serport 
  osap->addVPort(vPortSerial);
  // bus 
  ucBusDrop->init(false, BUS_DROP);
  // cl controller 
  step_cl->init();
Jake Read's avatar
Jake Read committed
57
58
  // start ctrl timer for 100khz loop, 
  d51_clock_boss->start_50kHz_ticker_tc0();
Jake Read's avatar
Jake Read committed
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
}

uint8_t bChPck[64];

volatile float current_floating_pos = 0.0F;
volatile int32_t current_step_pos = 0;
volatile uint32_t delta_steps = 0;

volatile float vel = 0.0F; 
volatile float move_counter = 0.0F;

volatile boolean setBlock = false;

uint16_t tick = 0;

Jake Read's avatar
Jake Read committed
74
75
76
77
78
79
80
void TC0_Handler(void){
  TC0->COUNT32.INTFLAG.bit.MC0 = 1;
  TC0->COUNT32.INTFLAG.bit.MC1 = 1;
  step_cl->run_torque_loop();
}

// async loop 
Jake Read's avatar
Jake Read committed
81
82
void loop() {
  osap->loop();
Jake Read's avatar
Jake Read committed
83
  step_a4950->dacRefresh();
Jake Read's avatar
Jake Read committed
84
  /*
Jake Read's avatar
Jake Read committed
85
86
87
88
89
90
91
92
  tick++;
  if(tick > 500){
    tick = 0;
    enc_as5047->trigger_read();
    while(!enc_as5047->is_read_complete());
    uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
    //sysError(String(reading));
  }
Jake Read's avatar
Jake Read committed
93
  */
Jake Read's avatar
Jake Read committed
94
95
96
97
98
99
100
101
102
103
104
105
106
107
  // do packet B grab / handle 
  /*
  if(ucBusDrop->ctr_b()){
    uint16_t len = ucBusDrop->read_b(bChPck);
    uint16_t ptr = 0;
    switch(bChPck[ptr]){
      case AK_SETCURRENT: {
        // get currents 
        ptr ++;
        chunk_float32 currentChunks[3];
        currentChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        currentChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        currentChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        // set current
Jake Read's avatar
Jake Read committed
108
        step_a4950->setCurrent(currentChunks[AXIS_PICK].f);
Jake Read's avatar
Jake Read committed
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
        // bug here 
        // I noticed that setcurrent commands to the z motor seem to fail: the motor drops to a low current no matter the value transmitted
        // I've checked (by inserting a flag to pick y-values) here, and those values work well, so it's nothing with the motor driver itself
        // so, I suspect the b-channel bus packets are coming out of those structures one byte short - makes sense there would be some bug in/around delineation 
        break;
        }
      case AK_SETPOS: {
        // get posns, 
        ptr ++;
        chunk_float32 posChunks[3];
        posChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        posChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        posChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        float target_current_pos = posChunks[AXIS_PICK].f;
        int32_t target_current_steps = lroundf(target_current_pos * SPU);
        setBlock = true; // don't do step work while these are modified 
        current_floating_pos = target_current_pos;
        current_step_pos = target_current_steps;
        setBlock = false;
      }
      default:
        // noop,
        break;
    }
  }
  */
} // end loop 

void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){
  // track end of header, to reply with 
  uint16_t replyPtr = ptr;
  // (a hack) store one app packet, to format our replies with. do once 
  if(replyBlankPtr == 0){
    for(uint16_t i = 0; i < pl; i++){
      replyBlankPck[i] = pck[i];
    }
    replyBlankPl = pl;
    replyBlankPtr = ptr;
    replyBlankSegsize = segsize;
    replyBlankVp = vp;
    replyBlankVpi = vpi;
  }
  // clear out our reply,   
  rl = 0;
  reply[rl ++] = DK_APP;
  // do the reading:
  ptr ++; // walk appcode DK_APP
  switch(pck[ptr]){
    case AK_RUNCALIB:
      ptr ++; // walk stepcode 
      reply[rl ++] = AK_RUNCALIB;
160
161
162
163
164
      if(step_cl->calibrate()){
        reply[rl ++] = AK_OK;
      } else {
        reply[rl ++] = AK_ERR;
      }
Jake Read's avatar
Jake Read committed
165
166
167
      // do step 
      break;
    case AK_READCALIB:
168
169
170
      ptr ++; // walk readcode
      reply[rl ++] = AK_READCALIB;
      step_cl->print_table(); 
Jake Read's avatar
Jake Read committed
171
172
      // do work 
      break;
Jake Read's avatar
Jake Read committed
173
174
175
176
177
178
179
    case AK_SET_TC: {
        ptr ++;
        reply[rl ++] = AK_SET_TC;
        chunk_float32 tcs = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] }};
        step_cl->set_torque(tcs.f);
        break;
      }
Jake Read's avatar
Jake Read committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
    default:
      break;
  }
  // end pck[ptr] switch 
  if(rl > 1){
    if(vp->cts()){
      appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl);
    } else {
      sysError("on reply, not cts, system fails");
    }
  }
  // always clear this 
  vp->clearPacket(pwp);
}

void UCBus_Drop::onRxISR(void){
  /*
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  move_counter += vel;
  boolean move_check = (move_counter > 1.0F);
  //DEBUG2PIN_TOGGLE;
  if(move_check){
    move_counter -= 1.0F;
    if(delta_steps == 0){
      // nothing 
    } else {
      //DEBUG1PIN_TOGGLE;
Jake Read's avatar
Jake Read committed
208
      step_a4950->step();
Jake Read's avatar
Jake Read committed
209
      delta_steps --;
Jake Read's avatar
Jake Read committed
210
      if(step_a4950->getDir()){
Jake Read's avatar
Jake Read committed
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
        current_step_pos ++;
      } else {
        current_step_pos --;
      }
    }
  }
  */
}

void UCBus_Drop::onPacketARx(void){
  /*
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  // last move is done, convert back steps -> float,
  current_floating_pos = current_step_pos / SPU;
  vel = 0.0F; // reset zero in case packet is not move 
  uint8_t bptr = 0;
  // switch bus packet types 
  switch(inBufferA[0]){
    case UB_AK_GOTOPOS:
      {
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        // chunk_float64 target = { 
        //   .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3],
        //             inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }};
        float delta = target.f - current_floating_pos;
        // update,
        //move_counter = 0.0F; // shouldn't need this ? try deleting 
        // direction swop,
        if(delta > 0){
Jake Read's avatar
Jake Read committed
244
          step_a4950->dir(true);
Jake Read's avatar
Jake Read committed
245
        } else {
Jake Read's avatar
Jake Read committed
246
          step_a4950->dir(false);
Jake Read's avatar
Jake Read committed
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
        }
        // how many steps, 
        delta_steps = lroundf(abs(delta * SPU));
        // what speed 
        vel = abs(delta * SPU) / TICKS_PER_PACKET;
        // for str8 r8 
        // if(delta_steps == 0){
        //   vel = 0.0F;
        // } else {
        //   vel = abs(delta * SPU) / TICKS_PER_PACKET;
        // }
        break; // end gotopos 
      }
    case UB_AK_SETPOS:
      {
        // reqest is to set position, not go to it... 
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        float target_current_pos = target.f;
        int32_t target_current_steps = lroundf(target_current_pos * SPU);
        setBlock = true; // don't do step work while these are modified 
        current_floating_pos = target_current_pos;
        current_step_pos = target_current_steps;
        vel = 0;
        delta_steps = 0;
        setBlock = false;
        break;
      }
    default:
      break;
  }  
  //DEBUG2PIN_OFF;
  */
}