servodisc goodness
main.cpp@10:4b7f2653fb45, 2018-02-04 (annotated)
- Committer:
- benkatz
- Date:
- Sun Feb 04 04:37:05 2018 +0000
- Revision:
- 10:4b7f2653fb45
- Parent:
- 9:61f214b91751
- Child:
- 11:16d807d6b9c5
update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 0:92d18e011d98 | 1 | #include "mbed.h" |
benkatz | 6:1143996ac690 | 2 | #include "cube.h" |
benkatz | 6:1143996ac690 | 3 | #include "communication.h" |
benkatz | 0:92d18e011d98 | 4 | |
benkatz | 1:27b535673eed | 5 | #define PI 3.14159265f |
benkatz | 1:27b535673eed | 6 | #define PWM_ARR 0x2E8 // PWM timer auto-reload |
benkatz | 1:27b535673eed | 7 | #define DT 0.00002067f // PWM_ARR/36 MHz |
benkatz | 1:27b535673eed | 8 | #define CPR 8000.0f // Encoder counts/revolution |
benkatz | 3:2e9713c61c2d | 9 | #define J 0.000065f // Inertia |
benkatz | 3:2e9713c61c2d | 10 | #define KT 0.0678f // Torque Constant |
benkatz | 1:27b535673eed | 11 | #define R 0.85f // Resistance |
benkatz | 9:61f214b91751 | 12 | #define V_IN 30.0f // DC input voltage |
benkatz | 3:2e9713c61c2d | 13 | #define K_SAT 22000.0f // Controller saturation gain |
benkatz | 3:2e9713c61c2d | 14 | #define DTC_MAX 0.97f // Max duty cycle (limited by bootstrapping) |
benkatz | 1:27b535673eed | 15 | #define V V_IN*DTC_MAX // Max useable voltage |
benkatz | 0:92d18e011d98 | 16 | |
benkatz | 3:2e9713c61c2d | 17 | #define TICKSTORAD(x) (float)x*2.0f*PI/CPR |
benkatz | 0:92d18e011d98 | 18 | #define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x))) |
benkatz | 0:92d18e011d98 | 19 | |
benkatz | 1:27b535673eed | 20 | Serial pc (PA_2, PA_3); // Serial to programming header |
benkatz | 1:27b535673eed | 21 | Serial io(PB_6, PB_7); // Differential Serial to JST Header |
benkatz | 6:1143996ac690 | 22 | DigitalIn id_3(PB_3); // ID Setting Jumpers |
benkatz | 1:27b535673eed | 23 | DigitalIn id_2(PB_4); |
benkatz | 6:1143996ac690 | 24 | DigitalIn id_1(PB_5); |
benkatz | 1:27b535673eed | 25 | DigitalOut led(PA_15); // Debug LED |
benkatz | 1:27b535673eed | 26 | DigitalIn d_in(PA_4); // LED on input from AND Board |
benkatz | 6:1143996ac690 | 27 | |
benkatz | 6:1143996ac690 | 28 | //AnalogOut a_out(PA_5); |
benkatz | 1:27b535673eed | 29 | DigitalOut d_out(PA_5); // LED on output to AND Board |
benkatz | 0:92d18e011d98 | 30 | |
benkatz | 0:92d18e011d98 | 31 | |
benkatz | 0:92d18e011d98 | 32 | void Control(); |
benkatz | 0:92d18e011d98 | 33 | void InitEncoder(); |
benkatz | 0:92d18e011d98 | 34 | void InitPWM(); |
benkatz | 1:27b535673eed | 35 | void InitGPIO(); |
benkatz | 0:92d18e011d98 | 36 | void WriteVoltage( float v); |
benkatz | 6:1143996ac690 | 37 | int ReadID(); |
benkatz | 6:1143996ac690 | 38 | |
benkatz | 4:6e290eb553cd | 39 | void SerialISR(); |
benkatz | 0:92d18e011d98 | 40 | |
benkatz | 0:92d18e011d98 | 41 | |
benkatz | 0:92d18e011d98 | 42 | |
benkatz | 0:92d18e011d98 | 43 | /* Control Variables */ |
benkatz | 1:27b535673eed | 44 | int id; |
benkatz | 3:2e9713c61c2d | 45 | int q_raw, dir, dq_raw = 0; |
benkatz | 3:2e9713c61c2d | 46 | float q, q_old, dq, u, e, q_ref, dqdebug = 0; |
benkatz | 3:2e9713c61c2d | 47 | int count, count2; |
benkatz | 6:1143996ac690 | 48 | int controlmode = 0; |
benkatz | 6:1143996ac690 | 49 | |
benkatz | 6:1143996ac690 | 50 | |
benkatz | 6:1143996ac690 | 51 | volatile int run_control = 0; |
benkatz | 6:1143996ac690 | 52 | volatile int position_setpoint = 0; |
benkatz | 3:2e9713c61c2d | 53 | |
benkatz | 3:2e9713c61c2d | 54 | /* Kalman Filter Variables */ |
benkatz | 3:2e9713c61c2d | 55 | float q_est[2] = {0.0f}; |
benkatz | 3:2e9713c61c2d | 56 | float q_meas[2] = {0.0f}; |
benkatz | 6:1143996ac690 | 57 | //float F[2][2] = {{1.0f, DT},{0.0f, 1.0f}}; |
benkatz | 6:1143996ac690 | 58 | //float B[2] = {0.0f, DT/J}; |
benkatz | 6:1143996ac690 | 59 | //float P[2][2] = {0}; |
benkatz | 6:1143996ac690 | 60 | //float Q[2] = {1.0f, 0.01f}; |
benkatz | 6:1143996ac690 | 61 | //float Rk[2] = {0.01, 10}; |
benkatz | 6:1143996ac690 | 62 | //float S[2][2] = {0}; |
benkatz | 6:1143996ac690 | 63 | //float Y[2] = {0}; |
benkatz | 6:1143996ac690 | 64 | //float K[2][2] = {0}; |
benkatz | 3:2e9713c61c2d | 65 | float U; |
benkatz | 3:2e9713c61c2d | 66 | |
benkatz | 3:2e9713c61c2d | 67 | |
benkatz | 5:96cd67bcac8c | 68 | //int8_t log_vec[1250] = {0}; |
benkatz | 3:2e9713c61c2d | 69 | //int16_t log_vec_2[1250] = {0}; |
benkatz | 3:2e9713c61c2d | 70 | |
benkatz | 0:92d18e011d98 | 71 | |
benkatz | 1:27b535673eed | 72 | /* PWM Timer Interrupt */ |
benkatz | 1:27b535673eed | 73 | extern "C" void TIM1_UP_TIM16_IRQHandler(void) { |
benkatz | 0:92d18e011d98 | 74 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 0:92d18e011d98 | 75 | } |
benkatz | 0:92d18e011d98 | 76 | count++; |
benkatz | 5:96cd67bcac8c | 77 | //d_out = !d_out; |
benkatz | 6:1143996ac690 | 78 | /* |
benkatz | 3:2e9713c61c2d | 79 | if(count>1000 && count<2000){ |
benkatz | 3:2e9713c61c2d | 80 | q_ref = 1.57f; |
benkatz | 3:2e9713c61c2d | 81 | //ref = 18000.0f; |
benkatz | 3:2e9713c61c2d | 82 | } |
benkatz | 3:2e9713c61c2d | 83 | |
benkatz | 3:2e9713c61c2d | 84 | if(count>2000 && count<3000){ |
benkatz | 3:2e9713c61c2d | 85 | q_ref = 0.0f; |
benkatz | 3:2e9713c61c2d | 86 | //ref = 0; |
benkatz | 3:2e9713c61c2d | 87 | //count = 0; |
benkatz | 3:2e9713c61c2d | 88 | } |
benkatz | 3:2e9713c61c2d | 89 | if(count>3000 && count<4000){ |
benkatz | 6:1143996ac690 | 90 | q_ref = 1.57f; |
benkatz | 3:2e9713c61c2d | 91 | } |
benkatz | 3:2e9713c61c2d | 92 | |
benkatz | 3:2e9713c61c2d | 93 | if(count>4500){ |
benkatz | 3:2e9713c61c2d | 94 | controlmode = 1; |
benkatz | 3:2e9713c61c2d | 95 | } |
benkatz | 6:1143996ac690 | 96 | |
benkatz | 3:2e9713c61c2d | 97 | if(count<5000){ |
benkatz | 3:2e9713c61c2d | 98 | //log_vec_2[count/4] = (int)(q_est[1]*10.0f); |
benkatz | 5:96cd67bcac8c | 99 | log_vec[count/4] = q_raw>>4; |
benkatz | 3:2e9713c61c2d | 100 | } |
benkatz | 3:2e9713c61c2d | 101 | |
benkatz | 5:96cd67bcac8c | 102 | if(count>20000 && count<21250){ |
benkatz | 5:96cd67bcac8c | 103 | printf("%d\n\r", log_vec[count2]); |
benkatz | 5:96cd67bcac8c | 104 | wait_us(80); |
benkatz | 3:2e9713c61c2d | 105 | //printf("%d\n\r", log_vec_2[count2]); |
benkatz | 3:2e9713c61c2d | 106 | //wait_us(200); |
benkatz | 3:2e9713c61c2d | 107 | count2++; |
benkatz | 3:2e9713c61c2d | 108 | } |
benkatz | 3:2e9713c61c2d | 109 | |
benkatz | 6:1143996ac690 | 110 | |
benkatz | 5:96cd67bcac8c | 111 | */ |
benkatz | 0:92d18e011d98 | 112 | Control(); |
benkatz | 5:96cd67bcac8c | 113 | |
benkatz | 10:4b7f2653fb45 | 114 | /* |
benkatz | 1:27b535673eed | 115 | if(count > 5000){ |
benkatz | 3:2e9713c61c2d | 116 | //io.printf("derp\n\r"); |
benkatz | 3:2e9713c61c2d | 117 | //pc.printf("derp\n\r"); |
benkatz | 5:96cd67bcac8c | 118 | pc.printf("%d \n\r", q_raw); |
benkatz | 5:96cd67bcac8c | 119 | //printf("%f %f\n\r", dq, dqdebug); |
benkatz | 3:2e9713c61c2d | 120 | //d_out = !d_out; |
benkatz | 0:92d18e011d98 | 121 | count = 0; |
benkatz | 0:92d18e011d98 | 122 | } |
benkatz | 10:4b7f2653fb45 | 123 | */ |
benkatz | 9:61f214b91751 | 124 | |
benkatz | 6:1143996ac690 | 125 | //a_out.write(q/2.0f); |
benkatz | 5:96cd67bcac8c | 126 | |
benkatz | 0:92d18e011d98 | 127 | TIM1->SR = 0x0; // reset the status register |
benkatz | 0:92d18e011d98 | 128 | } |
benkatz | 0:92d18e011d98 | 129 | |
benkatz | 6:1143996ac690 | 130 | // FUNCTIONS TO MODIFY |
benkatz | 6:1143996ac690 | 131 | int get_board_id() |
benkatz | 6:1143996ac690 | 132 | { |
benkatz | 6:1143996ac690 | 133 | return id; |
benkatz | 6:1143996ac690 | 134 | } |
benkatz | 6:1143996ac690 | 135 | |
benkatz | 6:1143996ac690 | 136 | void do_rotation(int8_t num_turns, int8_t derp) |
benkatz | 6:1143996ac690 | 137 | { |
benkatz | 6:1143996ac690 | 138 | printf("[BOARD %d] Rotate %d turns!\r\n",get_board_id(),num_turns); |
benkatz | 6:1143996ac690 | 139 | position_setpoint += num_turns; |
benkatz | 6:1143996ac690 | 140 | q_ref = 0.5f*PI*position_setpoint; |
benkatz | 6:1143996ac690 | 141 | run_control = 1; |
benkatz | 6:1143996ac690 | 142 | while(run_control) |
benkatz | 6:1143996ac690 | 143 | { |
benkatz | 6:1143996ac690 | 144 | ; |
benkatz | 6:1143996ac690 | 145 | } |
benkatz | 9:61f214b91751 | 146 | wait(0.001f); |
benkatz | 6:1143996ac690 | 147 | printf("done.\r\n"); |
benkatz | 6:1143996ac690 | 148 | } |
benkatz | 6:1143996ac690 | 149 | |
benkatz | 6:1143996ac690 | 150 | void set_and_board(int8_t value, int8_t derp) |
benkatz | 6:1143996ac690 | 151 | { |
benkatz | 6:1143996ac690 | 152 | printf("[BOARD %d] Set and board %d\r\n",get_board_id(),value); |
benkatz | 6:1143996ac690 | 153 | d_out = value; |
benkatz | 6:1143996ac690 | 154 | } |
benkatz | 6:1143996ac690 | 155 | |
benkatz | 6:1143996ac690 | 156 | int8_t get_and_board() |
benkatz | 6:1143996ac690 | 157 | { |
benkatz | 6:1143996ac690 | 158 | uint8_t value = d_in; |
benkatz | 6:1143996ac690 | 159 | //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value); |
benkatz | 6:1143996ac690 | 160 | return value; |
benkatz | 6:1143996ac690 | 161 | } |
benkatz | 6:1143996ac690 | 162 | |
benkatz | 6:1143996ac690 | 163 | |
benkatz | 6:1143996ac690 | 164 | |
benkatz | 6:1143996ac690 | 165 | // BEGIN STATE MACHINE CODE |
benkatz | 6:1143996ac690 | 166 | mbed_info_t state_machine_info; |
benkatz | 6:1143996ac690 | 167 | |
benkatz | 6:1143996ac690 | 168 | void state_machine_init() |
benkatz | 6:1143996ac690 | 169 | { |
benkatz | 6:1143996ac690 | 170 | printf("Start state machine!\r\n"); |
benkatz | 6:1143996ac690 | 171 | int board_id = get_board_id(); |
benkatz | 6:1143996ac690 | 172 | |
benkatz | 6:1143996ac690 | 173 | // set up state_machine_info |
benkatz | 6:1143996ac690 | 174 | state_machine_info.face = (face_t)board_id; |
benkatz | 6:1143996ac690 | 175 | pc.printf("\tboard id: %d\n",board_id); |
benkatz | 6:1143996ac690 | 176 | // set null sequence to disable everything |
benkatz | 6:1143996ac690 | 177 | state_machine_info.seq = NULL; |
benkatz | 6:1143996ac690 | 178 | state_machine_info.rotate = do_rotation; |
benkatz | 6:1143996ac690 | 179 | state_machine_info.set_and = set_and_board; |
benkatz | 6:1143996ac690 | 180 | state_machine_info.get_and = get_and_board; |
benkatz | 6:1143996ac690 | 181 | // prepare for serial |
benkatz | 6:1143996ac690 | 182 | clear_message_buffer(); |
benkatz | 6:1143996ac690 | 183 | } |
benkatz | 6:1143996ac690 | 184 | |
benkatz | 6:1143996ac690 | 185 | void handle_serial(Serial* pc) |
benkatz | 6:1143996ac690 | 186 | { |
benkatz | 6:1143996ac690 | 187 | while(pc->readable()) |
benkatz | 6:1143996ac690 | 188 | { |
benkatz | 6:1143996ac690 | 189 | // read it |
benkatz | 6:1143996ac690 | 190 | uint8_t data = pc->getc(); |
benkatz | 6:1143996ac690 | 191 | if(data == '\n') |
benkatz | 6:1143996ac690 | 192 | receive_move_sequence(pc,&state_machine_info); |
benkatz | 6:1143996ac690 | 193 | } |
benkatz | 6:1143996ac690 | 194 | } |
benkatz | 6:1143996ac690 | 195 | |
benkatz | 6:1143996ac690 | 196 | // END STATE MACHINE CODE |
benkatz | 3:2e9713c61c2d | 197 | |
benkatz | 0:92d18e011d98 | 198 | /* Main Loop */ |
benkatz | 0:92d18e011d98 | 199 | int main() { |
benkatz | 5:96cd67bcac8c | 200 | |
benkatz | 3:2e9713c61c2d | 201 | pc.baud(921600); |
benkatz | 6:1143996ac690 | 202 | io.baud(115200); |
benkatz | 3:2e9713c61c2d | 203 | |
benkatz | 6:1143996ac690 | 204 | pc.printf("\n\r Rubix Controller\n\r"); |
benkatz | 6:1143996ac690 | 205 | print_sample_sequence_hex(); |
benkatz | 1:27b535673eed | 206 | id_1.mode(PullUp); |
benkatz | 1:27b535673eed | 207 | id_2.mode(PullUp); |
benkatz | 1:27b535673eed | 208 | id_3.mode(PullUp); |
benkatz | 6:1143996ac690 | 209 | id = ReadID(); |
benkatz | 6:1143996ac690 | 210 | pc.printf(" Motor ID: %d\n\r", id); |
benkatz | 3:2e9713c61c2d | 211 | |
benkatz | 3:2e9713c61c2d | 212 | //d_in.mode(PullDown); |
benkatz | 1:27b535673eed | 213 | led = 1; |
benkatz | 6:1143996ac690 | 214 | d_out = 0; |
benkatz | 3:2e9713c61c2d | 215 | //wait(.1); |
benkatz | 1:27b535673eed | 216 | |
benkatz | 0:92d18e011d98 | 217 | InitPWM(); |
benkatz | 3:2e9713c61c2d | 218 | InitEncoder(); |
benkatz | 3:2e9713c61c2d | 219 | //pc.printf("Initializing Encoder\n\r"); |
benkatz | 3:2e9713c61c2d | 220 | //pc.printf("Initializing PWM\n\r"); |
benkatz | 3:2e9713c61c2d | 221 | //wait(.1); |
benkatz | 6:1143996ac690 | 222 | //io.attach(&SerialISR); |
benkatz | 6:1143996ac690 | 223 | |
benkatz | 6:1143996ac690 | 224 | reset_mbed(); //MUST call this first thing in main - initializes data structures! |
benkatz | 6:1143996ac690 | 225 | state_machine_init(); |
benkatz | 0:92d18e011d98 | 226 | while(1) { |
benkatz | 6:1143996ac690 | 227 | //myled = get_board_id(); |
benkatz | 6:1143996ac690 | 228 | handle_serial(&io); |
benkatz | 6:1143996ac690 | 229 | run_sequence_2(&state_machine_info); // run state machine |
benkatz | 0:92d18e011d98 | 230 | } |
benkatz | 0:92d18e011d98 | 231 | } |
benkatz | 0:92d18e011d98 | 232 | |
benkatz | 1:27b535673eed | 233 | /* Position Control */ |
benkatz | 0:92d18e011d98 | 234 | void Control(void){ |
benkatz | 3:2e9713c61c2d | 235 | |
benkatz | 3:2e9713c61c2d | 236 | // Sample Position and Velocity // |
benkatz | 3:2e9713c61c2d | 237 | q_raw = TIM2->CNT; |
benkatz | 3:2e9713c61c2d | 238 | dir = -2*(((TIM2->CR1)>>4)&1)+1; |
benkatz | 3:2e9713c61c2d | 239 | dq_raw = dir*(TIM15->CCR1); |
benkatz | 0:92d18e011d98 | 240 | q = TICKSTORAD(q_raw); |
benkatz | 3:2e9713c61c2d | 241 | //dq = (q - q_old)/DT; |
benkatz | 3:2e9713c61c2d | 242 | dq = (18000000.0f*4.0f*2.0f*PI/CPR)/((float)dq_raw); |
benkatz | 3:2e9713c61c2d | 243 | if(isinf(dq)){ dq = 0.0f;} |
benkatz | 0:92d18e011d98 | 244 | q_old = q; |
benkatz | 3:2e9713c61c2d | 245 | |
benkatz | 3:2e9713c61c2d | 246 | q_meas[0] = q; |
benkatz | 3:2e9713c61c2d | 247 | q_meas[1] = dq; |
benkatz | 3:2e9713c61c2d | 248 | |
benkatz | 3:2e9713c61c2d | 249 | // Kalman Filter // |
benkatz | 3:2e9713c61c2d | 250 | // Update Model // |
benkatz | 5:96cd67bcac8c | 251 | /* |
benkatz | 3:2e9713c61c2d | 252 | q_est[0] += q_est[1]*F[0][1]; |
benkatz | 3:2e9713c61c2d | 253 | q_est[1] += B[1]*U; |
benkatz | 3:2e9713c61c2d | 254 | |
benkatz | 3:2e9713c61c2d | 255 | |
benkatz | 3:2e9713c61c2d | 256 | P[0][0] += Q[0] + DT*P[1][0] + DT*(P[0][1] + DT*P[1][1]); |
benkatz | 3:2e9713c61c2d | 257 | P[0][1] += DT*P[1][1]; |
benkatz | 3:2e9713c61c2d | 258 | P[1][0] += DT*P[1][1]; |
benkatz | 3:2e9713c61c2d | 259 | P[1][1] += Q[1]; |
benkatz | 3:2e9713c61c2d | 260 | |
benkatz | 3:2e9713c61c2d | 261 | //Calculate Kalman Gains// |
benkatz | 3:2e9713c61c2d | 262 | S[0][0] = P[0][0] + Rk[0]; |
benkatz | 3:2e9713c61c2d | 263 | S[0][1] = P[0][1]; |
benkatz | 3:2e9713c61c2d | 264 | S[1][0] = P[1][0]; |
benkatz | 3:2e9713c61c2d | 265 | S[1][1] = P[1][1] + Rk[1]; |
benkatz | 3:2e9713c61c2d | 266 | float denom = (S[0][0]*S[1][1] - S[0][1]*S[1][0]); |
benkatz | 3:2e9713c61c2d | 267 | K[0][0] = (P[0][0]*S[1][1])/denom - (P[0][1]*S[1][0])/denom; |
benkatz | 3:2e9713c61c2d | 268 | K[0][1] = (P[0][1]*S[0][0])/denom - (P[0][0]*S[0][1])/denom; |
benkatz | 3:2e9713c61c2d | 269 | K[1][0] = (P[1][0]*S[1][1])/(S[0][0]*S[1][1] - S[0][1]*S[1][0]) - (P[1][1]*S[1][0])/denom; |
benkatz | 3:2e9713c61c2d | 270 | K[1][1] = (P[1][1]*S[0][0])/(S[0][0]*S[1][1] - S[0][1]*S[1][0]) - (P[1][0]*S[0][1])/denom; |
benkatz | 3:2e9713c61c2d | 271 | |
benkatz | 3:2e9713c61c2d | 272 | Y[0] = q_meas[0] - q_est[0]; |
benkatz | 3:2e9713c61c2d | 273 | Y[1] = q_meas[1] - q_est[1]; |
benkatz | 3:2e9713c61c2d | 274 | |
benkatz | 3:2e9713c61c2d | 275 | // Update Estimate // |
benkatz | 3:2e9713c61c2d | 276 | q_est[0] += K[0][0]*Y[0] + K[0][1]*Y[1]; |
benkatz | 3:2e9713c61c2d | 277 | q_est[1] += K[1][0]*Y[0] + K[1][1]*Y[1]; |
benkatz | 3:2e9713c61c2d | 278 | |
benkatz | 3:2e9713c61c2d | 279 | P[0][0] = -K[0][1]*P[1][0] - P[0][0]*(K[0][0] - 1.0f); |
benkatz | 3:2e9713c61c2d | 280 | P[0][1] = -K[0][1]*P[1][1] - P[0][1]*(K[0][0] - 1.0f); |
benkatz | 3:2e9713c61c2d | 281 | P[1][0] = -K[1][0]*P[0][0] - P[1][0]*(K[1][1] - 1.0f); |
benkatz | 3:2e9713c61c2d | 282 | P[1][1] = -K[1][0]*P[0][1] - P[1][1]*(K[1][1] - 1.0f); |
benkatz | 5:96cd67bcac8c | 283 | */ |
benkatz | 3:2e9713c61c2d | 284 | |
benkatz | 3:2e9713c61c2d | 285 | |
benkatz | 3:2e9713c61c2d | 286 | // Control Law // |
benkatz | 6:1143996ac690 | 287 | if(run_control == 1){ |
benkatz | 3:2e9713c61c2d | 288 | e = K_SAT*((q_ref - q) + (abs(dq)*dq*1.3f*R*J)/(2.0f*KT*(-V - KT*abs(dq)))); // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response |
benkatz | 3:2e9713c61c2d | 289 | //e = K_SAT*((q_ref - q) + (abs(q_est[1])*q_est[1]*1.3f*R*J)/(2.0f*KT*(-V - 1.0f*KT*abs(q_est[1])))); // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response |
benkatz | 3:2e9713c61c2d | 290 | |
benkatz | 3:2e9713c61c2d | 291 | } |
benkatz | 6:1143996ac690 | 292 | |
benkatz | 3:2e9713c61c2d | 293 | //q_ref = 0.0f; |
benkatz | 6:1143996ac690 | 294 | if(run_control == 0){ |
benkatz | 8:25a28a2ac486 | 295 | //e = 0; |
benkatz | 8:25a28a2ac486 | 296 | e = 100.0f*(q_ref - q) + .25f*(0.0f-dq); |
benkatz | 3:2e9713c61c2d | 297 | } |
benkatz | 6:1143996ac690 | 298 | if(run_control&(abs(q_ref - q))<.05f){ |
benkatz | 6:1143996ac690 | 299 | printf("control done\n\r");} |
benkatz | 6:1143996ac690 | 300 | run_control = (abs(q_ref - q))>.05f; |
benkatz | 6:1143996ac690 | 301 | |
benkatz | 0:92d18e011d98 | 302 | u = CONSTRAIN(e, -V, V); |
benkatz | 3:2e9713c61c2d | 303 | WriteVoltage(u); |
benkatz | 3:2e9713c61c2d | 304 | U = KT*(u - KT*dq)/R; |
benkatz | 3:2e9713c61c2d | 305 | //WriteVoltage(-10.0f); |
benkatz | 1:27b535673eed | 306 | } |
benkatz | 1:27b535673eed | 307 | |
benkatz | 1:27b535673eed | 308 | /* Set motor voltage */ |
benkatz | 1:27b535673eed | 309 | void WriteVoltage(float v){ |
benkatz | 1:27b535673eed | 310 | if(v>0){ |
benkatz | 1:27b535673eed | 311 | TIM1->CCR1 = 0; |
benkatz | 1:27b535673eed | 312 | TIM1->CCR2 = (int) (PWM_ARR*(v/V)); |
benkatz | 1:27b535673eed | 313 | } |
benkatz | 3:2e9713c61c2d | 314 | else if(v<=0){ |
benkatz | 1:27b535673eed | 315 | TIM1->CCR2 = 0; |
benkatz | 3:2e9713c61c2d | 316 | TIM1->CCR1 = (int) (PWM_ARR*(abs(v)/V)); |
benkatz | 1:27b535673eed | 317 | } |
benkatz | 0:92d18e011d98 | 318 | } |
benkatz | 0:92d18e011d98 | 319 | |
benkatz | 3:2e9713c61c2d | 320 | void SerialISR(void){ |
benkatz | 3:2e9713c61c2d | 321 | |
benkatz | 4:6e290eb553cd | 322 | io.putc(io.getc()); |
benkatz | 3:2e9713c61c2d | 323 | |
benkatz | 3:2e9713c61c2d | 324 | } |
benkatz | 3:2e9713c61c2d | 325 | |
benkatz | 3:2e9713c61c2d | 326 | |
benkatz | 1:27b535673eed | 327 | /* Read ID Jumpers */ |
benkatz | 6:1143996ac690 | 328 | int ReadID(void){ |
benkatz | 1:27b535673eed | 329 | int i1 = !id_1.read(); |
benkatz | 1:27b535673eed | 330 | int i2 = !id_2.read(); |
benkatz | 1:27b535673eed | 331 | int i3 = !id_3.read(); |
benkatz | 1:27b535673eed | 332 | return (i1<<2) | (i2<<1) | i3; |
benkatz | 0:92d18e011d98 | 333 | } |
benkatz | 0:92d18e011d98 | 334 | |
benkatz | 6:1143996ac690 | 335 | |
benkatz | 1:27b535673eed | 336 | /* Initialize Encoder */ |
benkatz | 0:92d18e011d98 | 337 | void InitEncoder(void) { |
benkatz | 0:92d18e011d98 | 338 | // configure GPIO PA0 & PA1 as inputs for Encoder |
benkatz | 3:2e9713c61c2d | 339 | //RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA |
benkatz | 1:27b535673eed | 340 | GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; // PA0 & PA1 as Alternate Function |
benkatz | 1:27b535673eed | 341 | GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; // PA0 & PA1 as Inputs |
benkatz | 1:27b535673eed | 342 | GPIOA->OSPEEDR |= 0x00000011; // GPIO Speed |
benkatz | 3:2e9713c61c2d | 343 | //GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down |
benkatz | 1:27b535673eed | 344 | GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 |
benkatz | 1:27b535673eed | 345 | GPIOA->AFR[1] |= 0x00000000 ; // |
benkatz | 1:27b535673eed | 346 | |
benkatz | 0:92d18e011d98 | 347 | // configure TIM2 as Encoder input |
benkatz | 3:2e9713c61c2d | 348 | TIM2->DIER = 0x00; |
benkatz | 3:2e9713c61c2d | 349 | TIM2->EGR = 0x0; |
benkatz | 3:2e9713c61c2d | 350 | NVIC_DisableIRQ(TIM2_IRQn); |
benkatz | 3:2e9713c61c2d | 351 | |
benkatz | 1:27b535673eed | 352 | RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 |
benkatz | 1:27b535673eed | 353 | TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' |
benkatz | 1:27b535673eed | 354 | TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) |
benkatz | 1:27b535673eed | 355 | TIM2->CCMR1 = 0x5151; // CC1S='01' CC2S='01' |
benkatz | 1:27b535673eed | 356 | TIM2->CCMR2 = 0x0000; |
benkatz | 1:27b535673eed | 357 | TIM2->CCER = 0x0011; // CC1P CC2P |
benkatz | 1:27b535673eed | 358 | TIM2->PSC = 0x0000; // Prescaler = (0+1) |
benkatz | 1:27b535673eed | 359 | TIM2->CNT = 0x0000; //reset the counter before we use it |
benkatz | 3:2e9713c61c2d | 360 | |
benkatz | 3:2e9713c61c2d | 361 | TIM2->CR2 = 0x030; //MMS = 101 |
benkatz | 3:2e9713c61c2d | 362 | __TIM15_CLK_ENABLE(); |
benkatz | 3:2e9713c61c2d | 363 | TIM15->PSC = 0x03; |
benkatz | 3:2e9713c61c2d | 364 | TIM15->SMCR = 0x4; //TS = 010 for ITR2, SMS = 100 |
benkatz | 3:2e9713c61c2d | 365 | TIM15->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC |
benkatz | 3:2e9713c61c2d | 366 | TIM15->CCER |= TIM_CCER_CC1P; |
benkatz | 3:2e9713c61c2d | 367 | TIM15->CCER |= TIM_CCER_CC1E; |
benkatz | 3:2e9713c61c2d | 368 | TIM15->CR1 = 0x1; |
benkatz | 3:2e9713c61c2d | 369 | |
benkatz | 0:92d18e011d98 | 370 | } |
benkatz | 0:92d18e011d98 | 371 | |
benkatz | 3:2e9713c61c2d | 372 | |
benkatz | 1:27b535673eed | 373 | /* Initialize PWM */ |
benkatz | 0:92d18e011d98 | 374 | void InitPWM(void){ |
benkatz | 1:27b535673eed | 375 | RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA |
benkatz | 1:27b535673eed | 376 | RCC->AHBENR |= RCC_AHBENR_GPIOBEN; // enable the clock to GPIOB |
benkatz | 1:27b535673eed | 377 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock |
benkatz | 0:92d18e011d98 | 378 | |
benkatz | 0:92d18e011d98 | 379 | GPIOA->MODER |= GPIO_MODER_MODER7_1 | GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 ; //PA_7, PA_8, PA_9 to alternate funtion mode |
benkatz | 0:92d18e011d98 | 380 | GPIOB->MODER |= GPIO_MODER_MODER0_1; // PB_0 to alternate function mode |
benkatz | 0:92d18e011d98 | 381 | GPIOA->AFR[0] |= 0x60000000; // PA_7 to alternate function 6 |
benkatz | 0:92d18e011d98 | 382 | GPIOA->AFR[1] |= 0x00000066; // PA_8, PA_9 to alternate function 6 |
benkatz | 0:92d18e011d98 | 383 | GPIOB->AFR[0] |= 0x00000006; // PB_0 to alternate function 6 |
benkatz | 0:92d18e011d98 | 384 | |
benkatz | 0:92d18e011d98 | 385 | //PWM Setup |
benkatz | 0:92d18e011d98 | 386 | TIM1->CCMR1 |= 0x6060; // Enable output compare 1 and 2 |
benkatz | 0:92d18e011d98 | 387 | TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC2E; // enable outputs 1, 2, and complementary outputs |
benkatz | 1:27b535673eed | 388 | TIM1->BDTR |= TIM_BDTR_MOE | 0xF; // MOE = 1 | set dead-time |
benkatz | 1:27b535673eed | 389 | TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock |
benkatz | 1:27b535673eed | 390 | TIM1->ARR = PWM_ARR; // set auto reload |
benkatz | 1:27b535673eed | 391 | TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on, |
benkatz | 1:27b535673eed | 392 | TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 |
benkatz | 0:92d18e011d98 | 393 | |
benkatz | 3:2e9713c61c2d | 394 | |
benkatz | 1:27b535673eed | 395 | NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); //Enable TIM1 IRQ |
benkatz | 1:27b535673eed | 396 | TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt |
benkatz | 1:27b535673eed | 397 | TIM1->CR1 |= 0x40; //CMS = 10, interrupt only when counting up |
benkatz | 1:27b535673eed | 398 | TIM1->RCR |= 0x001; // update event once per up/down count of tim1 |
benkatz | 3:2e9713c61c2d | 399 | TIM1->EGR |= TIM_EGR_UG; |
benkatz | 3:2e9713c61c2d | 400 | |
benkatz | 3:2e9713c61c2d | 401 | } |
benkatz | 3:2e9713c61c2d | 402 |