Ben Katz / rubix_controller

servodisc goodness

Dependencies:   mbed-dev-f303

main.cpp@11:16d807d6b9c5, 2018-03-07 (annotated)

Committer:
benkatz
Date:
Wed Mar 07 19:25:49 2018 +0000
Revision:
11:16d807d6b9c5
Parent:
10:4b7f2653fb45 
Seems to work;

Who changed what in which revision?

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