Rev 1665 | Rev 1672 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1665 | Rev 1666 | ||
|---|---|---|---|
| Line 120... | Line 120... | ||
| 120 | { |
120 | { |
| 121 | Motor[i].Version = 0; |
121 | Motor[i].Version = 0; |
| 122 | Motor[i].SetPoint = 0; |
122 | Motor[i].SetPoint = 0; |
| 123 | Motor[i].SetPointLowerBits = 0; |
123 | Motor[i].SetPointLowerBits = 0; |
| 124 | Motor[i].State = 0; |
124 | Motor[i].State = 0; |
| - | 125 | Motor[i].ReadMode = BL_READMODE_STATUS; |
|
| 125 | Motor[i].Current = 0; |
126 | Motor[i].Current = 0; |
| 126 | Motor[i].MaxPWM = 0; |
127 | Motor[i].MaxPWM = 0; |
| - | 128 | Motor[i].Temperature = 0; |
|
| 127 | } |
129 | } |
| Line 128... | Line 130... | ||
| 128 | 130 | ||
| 129 | SREG = sreg; |
131 | SREG = sreg; |
| Line 153... | Line 155... | ||
| 153 | { |
155 | { |
| 154 | static uint8_t missing_motor = 0, motor_read_temperature = 0; |
156 | static uint8_t missing_motor = 0, motor_read_temperature = 0; |
| 155 | static uint8_t *pBuff = 0; |
157 | static uint8_t *pBuff = 0; |
| 156 | static uint8_t BuffLen = 0; |
158 | static uint8_t BuffLen = 0; |
| Line 157... | Line -... | ||
| 157 | - | ||
| 158 | #define BL_READ_STATUS 0 |
- | |
| 159 | #define BL_READ_CONFIG 16 |
- | |
| 160 | static uint8_t BLReadMode = BL_READ_STATUS; |
- | |
| 161 | 159 | ||
| 162 | switch (twi_state++) |
160 | switch (twi_state++) |
| 163 | { |
161 | { |
| 164 | // Master Transmit |
162 | // Master Transmit |
| Line 187... | Line 185... | ||
| 187 | { // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
185 | { // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
| 188 | twi_state = 4; //jump over sending more data |
186 | twi_state = 4; //jump over sending more data |
| 189 | } |
187 | } |
| 190 | break; |
188 | break; |
| 191 | case 2: // lower bits of setpoint (higher resolution) |
189 | case 2: // lower bits of setpoint (higher resolution) |
| 192 | if ((motor_write == motor_read) && ((0x0001<<motor_read) & BLConfig_ReadMask)) |
190 | if ((0x0001<<motor_write) & BLConfig_ReadMask) |
| 193 | { |
191 | { |
| 194 | BLReadMode = BL_READ_CONFIG; // configuration request |
192 | Motor[motor_write].ReadMode = BL_READMODE_CONFIG; // configuration request |
| 195 | } |
193 | } |
| 196 | else |
194 | else |
| 197 | { |
195 | { |
| 198 | BLReadMode = BL_READ_STATUS; // normal status request |
196 | Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request |
| 199 | } |
197 | } |
| 200 | // send read mode and the lower bits of setpoint |
198 | // send read mode and the lower bits of setpoint |
| 201 | I2C_WriteByte((BLReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
199 | I2C_WriteByte((Motor[motor_write].ReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
| 202 | // configuration tranmission request? |
200 | // configuration tranmission request? |
| 203 | if((0x0001<<motor_write) & BLConfig_WriteMask) |
201 | if((0x0001<<motor_write) & BLConfig_WriteMask) |
| 204 | { // redirect tx pointer to configuration data |
202 | { // redirect tx pointer to configuration data |
| 205 | pBuff = (uint8_t*)&BLConfig; // select config for motor |
203 | pBuff = (uint8_t*)&BLConfig; // select config for motor |
| 206 | BuffLen = sizeof(BLConfig_t); |
204 | BuffLen = sizeof(BLConfig_t); |
| Line 249... | Line 247... | ||
| 249 | Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
247 | Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
| Line 250... | Line 248... | ||
| 250 | 248 | ||
| 251 | if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
249 | if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
| 252 | { |
250 | { |
| 253 | // new BL found |
251 | // new BL found |
| 254 | switch(BLReadMode) |
252 | switch(Motor[motor_read].ReadMode) |
| 255 | { |
253 | { |
| 256 | case BL_READ_CONFIG: |
254 | case BL_READMODE_CONFIG: |
| 257 | pBuff = (uint8_t*)&BLConfig; |
255 | pBuff = (uint8_t*)&BLConfig; |
| 258 | BuffLen = sizeof(BLConfig_t); |
256 | BuffLen = sizeof(BLConfig_t); |
| Line 259... | Line 257... | ||
| 259 | break; |
257 | break; |
| 260 | 258 | ||
| 261 | case BL_READ_STATUS: |
259 | case BL_READMODE_STATUS: |
| 262 | pBuff = (uint8_t*)&(Motor[motor_read].Current); |
260 | pBuff = (uint8_t*)&(Motor[motor_read].Current); |
| 263 | if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
261 | if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
| 264 | else BuffLen = 1;// read Current only |
262 | else BuffLen = 1;// read Current only |