dikuei yen / Mbed 2 deprecated MTi_Gss_Motor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:fdfd9a35acc4, 2022-05-02 (annotated)

Committer:
dikueiyen
Date:
Mon May 02 11:11:20 2022 +0000
Revision:
1:fdfd9a35acc4
Parent:
0:6bee2baac968
Child:
3:d4736e223540
20220502

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dikueiyen 0:6bee2baac968 1 //20211018 version
dikueiyen 0:6bee2baac968 2 #include "MTi2.h"
dikueiyen 0:6bee2baac968 3 #include <stdio.h>
dikueiyen 0:6bee2baac968 4 #include "mbed.h"
dikueiyen 0:6bee2baac968 5 #include <math.h>
dikueiyen 0:6bee2baac968 6 #include <stdlib.h>
dikueiyen 0:6bee2baac968 7 #include "PMW3901.h"
dikueiyen 0:6bee2baac968 8
dikueiyen 0:6bee2baac968 9 #define GRAVITYACCELERATION 9.81f
dikueiyen 0:6bee2baac968 10 #define pi 3.14159265358979323846f
dikueiyen 0:6bee2baac968 11 //motor
dikueiyen 0:6bee2baac968 12 #define maximum_volt 12.0f
dikueiyen 0:6bee2baac968 13 #define minimum_volt 0.45f // Need to test for different loads.
dikueiyen 0:6bee2baac968 14
dikueiyen 0:6bee2baac968 15 #define INPUT_VOLTAGE 12.5f
dikueiyen 0:6bee2baac968 16 #define PWM_FREQUENCY 10.0f // the default value we set is 20.0 (unit : kHz)
dikueiyen 0:6bee2baac968 17 #define PWM_STOP 0.5f //the pwm dutycycle value is from 0~1 and 0.5 can let motor stop
dikueiyen 0:6bee2baac968 18
dikueiyen 0:6bee2baac968 19 #define FRICTION_VOLTAGE 0.45f
dikueiyen 0:6bee2baac968 20 #define HALL_RESOLUTION 64.0f
dikueiyen 0:6bee2baac968 21 #define GEAR_RATIO 56.0f
dikueiyen 0:6bee2baac968 22 #define VOLT_CMD 8.0f // unit(voltage)
dikueiyen 0:6bee2baac968 23
dikueiyen 0:6bee2baac968 24 //Common
dikueiyen 0:6bee2baac968 25 Serial pc(USBTX,USBRX);
dikueiyen 0:6bee2baac968 26 InterruptIn mybutton(USER_BUTTON);
dikueiyen 0:6bee2baac968 27 Ticker main_function; //interrupt
dikueiyen 0:6bee2baac968 28 DigitalOut led1(LED1);
dikueiyen 0:6bee2baac968 29 DigitalOut led2(A4);
dikueiyen 0:6bee2baac968 30 DigitalOut led3(A5);
dikueiyen 0:6bee2baac968 31
dikueiyen 0:6bee2baac968 32 //IMU
dikueiyen 0:6bee2baac968 33 SPI spi_MTI(PB_15, PB_14, PB_13);//MOSI MISO SCLK
dikueiyen 0:6bee2baac968 34 DigitalOut cs_MTI(PC_4);
dikueiyen 0:6bee2baac968 35
dikueiyen 0:6bee2baac968 36 //GSS
dikueiyen 0:6bee2baac968 37 SPI spi(PC_12,PC_11,PC_10);
dikueiyen 0:6bee2baac968 38 DigitalOut cs(PA_4);
dikueiyen 0:6bee2baac968 39
dikueiyen 0:6bee2baac968 40 //motor
dikueiyen 0:6bee2baac968 41 PwmOut pwm1A(D7);
dikueiyen 0:6bee2baac968 42 PwmOut pwm1B(D8);
dikueiyen 0:6bee2baac968 43 PwmOut pwm2A(D11);
dikueiyen 0:6bee2baac968 44 PwmOut pwm2B(A3);
dikueiyen 0:6bee2baac968 45
dikueiyen 0:6bee2baac968 46 //Common
dikueiyen 0:6bee2baac968 47 bool button_state = false;
dikueiyen 0:6bee2baac968 48 float dt = 0.01; // sec
dikueiyen 0:6bee2baac968 49 //IMU
dikueiyen 0:6bee2baac968 50 typedef union{
dikueiyen 0:6bee2baac968 51 uint32_t data1;
dikueiyen 0:6bee2baac968 52 float data2;
dikueiyen 0:6bee2baac968 53 }imu_data;
dikueiyen 0:6bee2baac968 54
dikueiyen 0:6bee2baac968 55 imu_data eul[3];//euler angle
dikueiyen 0:6bee2baac968 56 imu_data acc[3];
dikueiyen 0:6bee2baac968 57 imu_data gry[3];
dikueiyen 0:6bee2baac968 58 //GSS
dikueiyen 0:6bee2baac968 59 int GSS_X = 0;
dikueiyen 0:6bee2baac968 60 int GSS_Y = 0;
dikueiyen 0:6bee2baac968 61
dikueiyen 0:6bee2baac968 62 //motor
dikueiyen 0:6bee2baac968 63 int readcount = 0;
dikueiyen 0:6bee2baac968 64 int RX_flag2 = 0;
dikueiyen 0:6bee2baac968 65 char getData[6] = {0,0,0,0,0,0};
dikueiyen 0:6bee2baac968 66 short data_received[2] = {0,0};
dikueiyen 0:6bee2baac968 67
dikueiyen 0:6bee2baac968 68 float command = 0;
dikueiyen 0:6bee2baac968 69 float velocityA = 0; //rpm
dikueiyen 0:6bee2baac968 70 float velocityB = 0;
dikueiyen 0:6bee2baac968 71 float positionA = 0;
dikueiyen 0:6bee2baac968 72 float positionB = 0;
dikueiyen 0:6bee2baac968 73 short EncoderPositionA;
dikueiyen 0:6bee2baac968 74 short EncoderPositionB;
dikueiyen 0:6bee2baac968 75 float last_voltA = 0;
dikueiyen 0:6bee2baac968 76 float last_voltB = 0;
dikueiyen 0:6bee2baac968 77 float errorA = 0;
dikueiyen 0:6bee2baac968 78 float error_drA = 0;
dikueiyen 0:6bee2baac968 79 float errorB = 0;
dikueiyen 0:6bee2baac968 80 float error_drB = 0;
dikueiyen 0:6bee2baac968 81 float dutycycle = PWM_STOP;
dikueiyen 0:6bee2baac968 82 float VELOCITY_SPEED_A = 0.0;
dikueiyen 0:6bee2baac968 83 float VELOCITY_SPEED_B = 0.0;
dikueiyen 0:6bee2baac968 84 int pub_count = 0;
dikueiyen 0:6bee2baac968 85
dikueiyen 0:6bee2baac968 86 //Common
dikueiyen 0:6bee2baac968 87 void step_command();
dikueiyen 0:6bee2baac968 88
dikueiyen 0:6bee2baac968 89 //IMU
dikueiyen 0:6bee2baac968 90 void Start_read();
dikueiyen 0:6bee2baac968 91
dikueiyen 0:6bee2baac968 92 //GSS
dikueiyen 0:6bee2baac968 93 void grabData(void);
dikueiyen 0:6bee2baac968 94 //void printData(void);
dikueiyen 0:6bee2baac968 95 void initializeSensor(void);
dikueiyen 0:6bee2baac968 96 void writeRegister(uint8_t addr, uint8_t data);
dikueiyen 0:6bee2baac968 97 uint8_t readRegister(uint8_t addr);
dikueiyen 0:6bee2baac968 98 void delayus(uint32_t us);
dikueiyen 0:6bee2baac968 99
dikueiyen 0:6bee2baac968 100 //motor
dikueiyen 0:6bee2baac968 101 float PD(float e, float last_e, float last_u, float P, float D);
dikueiyen 0:6bee2baac968 102 float PDF(float e, float last_e, float last_u, float P, float D, float F);
dikueiyen 0:6bee2baac968 103 void ReadVelocity();
dikueiyen 0:6bee2baac968 104 void ReadPosition(float *positionA, float *positionB);
dikueiyen 0:6bee2baac968 105 void motor_drive(float voltA, float voltB);
dikueiyen 0:6bee2baac968 106 void InitMotor(float pwm_frequency);
dikueiyen 0:6bee2baac968 107 void InitEncoder(void);
dikueiyen 0:6bee2baac968 108 void control_speed();
dikueiyen 0:6bee2baac968 109
dikueiyen 0:6bee2baac968 110 void RX_ITR();
dikueiyen 0:6bee2baac968 111 void init_UART();
dikueiyen 0:6bee2baac968 112
dikueiyen 0:6bee2baac968 113 int main(void)
dikueiyen 0:6bee2baac968 114 {
dikueiyen 0:6bee2baac968 115 pc.baud(230400);
dikueiyen 0:6bee2baac968 116 led2 = 1;
dikueiyen 0:6bee2baac968 117 led3 = 1;
dikueiyen 0:6bee2baac968 118 //IMU
dikueiyen 0:6bee2baac968 119 spi_MTI.format(8, 3);
dikueiyen 0:6bee2baac968 120 MTi2_Init();
dikueiyen 0:6bee2baac968 121 //GSS
dikueiyen 0:6bee2baac968 122 spi.format(8, 3);
dikueiyen 0:6bee2baac968 123 initializeSensor();
dikueiyen 0:6bee2baac968 124 //motor
dikueiyen 0:6bee2baac968 125 init_UART();
dikueiyen 0:6bee2baac968 126 InitEncoder();
dikueiyen 0:6bee2baac968 127 InitMotor(PWM_FREQUENCY);
dikueiyen 0:6bee2baac968 128
dikueiyen 0:6bee2baac968 129 mybutton.fall(&step_command);
dikueiyen 0:6bee2baac968 130 main_function.attach_us(&Start_read,dt*1000000);
dikueiyen 0:6bee2baac968 131 while (1) {
dikueiyen 0:6bee2baac968 132 }
dikueiyen 0:6bee2baac968 133
dikueiyen 0:6bee2baac968 134 }
dikueiyen 0:6bee2baac968 135
dikueiyen 0:6bee2baac968 136 void Start_read() //interrupt function by TT
dikueiyen 0:6bee2baac968 137 {
dikueiyen 0:6bee2baac968 138 //IMU
dikueiyen 0:6bee2baac968 139 ReadData();
dikueiyen 0:6bee2baac968 140 //GSS
dikueiyen 0:6bee2baac968 141 cs = 0;
dikueiyen 0:6bee2baac968 142 grabData();
dikueiyen 0:6bee2baac968 143 cs = 1;
dikueiyen 0:6bee2baac968 144 if(button_state == true){
dikueiyen 0:6bee2baac968 145 pub_count++;
dikueiyen 0:6bee2baac968 146 // VELOCITY_SPEED_A = -10.0f;
dikueiyen 0:6bee2baac968 147 // VELOCITY_SPEED_B = -10.0f;
dikueiyen 0:6bee2baac968 148 ReadVelocity();
dikueiyen 0:6bee2baac968 149 control_speed();
dikueiyen 0:6bee2baac968 150 if (pub_count >= 10){
dikueiyen 0:6bee2baac968 151 //printf("%.3f,%.3f\r\n",velocityA, velocityB); // velocityA or velocityB
dikueiyen 0:6bee2baac968 152 //printf("CMD %.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
dikueiyen 0:6bee2baac968 153 pub_count = 0;
dikueiyen 0:6bee2baac968 154 }
dikueiyen 0:6bee2baac968 155 }else{
dikueiyen 0:6bee2baac968 156 uint16_t dutycycleA = PWM_STOP *uint16_t(TIM1->ARR);
dikueiyen 0:6bee2baac968 157 uint16_t dutycycleB = PWM_STOP *uint16_t(TIM1->ARR);
dikueiyen 0:6bee2baac968 158 TIM1->CCR1 = dutycycleA;
dikueiyen 0:6bee2baac968 159 TIM1->CCR2 = dutycycleB;
dikueiyen 0:6bee2baac968 160 command = 0;
dikueiyen 0:6bee2baac968 161 }
dikueiyen 0:6bee2baac968 162 if (button_state == true)
dikueiyen 0:6bee2baac968 163 {
dikueiyen 0:6bee2baac968 164
dikueiyen 0:6bee2baac968 165 // printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],accel_[0],accel_[1],accel_[2],omega[0],omega[1],omega[2]);
dikueiyen 0:6bee2baac968 166 // printf("%d,%d\n\r", GSS_X, GSS_Y);
dikueiyen 0:6bee2baac968 167 // printf("%.3f,%.3f\r\n",velocityA, velocityB); // velocityA or velocityB
dikueiyen 0:6bee2baac968 168
dikueiyen 0:6bee2baac968 169 printf("%.3f,%.3f,%d,%d,%.4f,%.4f,%.4f\r\n",velocityA, velocityB, GSS_X, GSS_Y, accel_[0],accel_[1], omega[2]);
dikueiyen 0:6bee2baac968 170 // printf("%.2f,%.2f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
dikueiyen 0:6bee2baac968 171 }
dikueiyen 0:6bee2baac968 172 }
dikueiyen 0:6bee2baac968 173
dikueiyen 0:6bee2baac968 174
dikueiyen 0:6bee2baac968 175 void step_command(){
dikueiyen 0:6bee2baac968 176 led1 = !led1;
dikueiyen 0:6bee2baac968 177 led2 = !led2;
dikueiyen 0:6bee2baac968 178 led3 = !led3;
dikueiyen 0:6bee2baac968 179 button_state = !button_state;
dikueiyen 0:6bee2baac968 180 }
dikueiyen 0:6bee2baac968 181 //IMU
dikueiyen 0:6bee2baac968 182 void SendOpcode(uint8_t Opcode)
dikueiyen 0:6bee2baac968 183 {
dikueiyen 0:6bee2baac968 184 // printf("SendOpcode \r\n");
dikueiyen 0:6bee2baac968 185 FW[0] = spi_MTI.write(Opcode);
dikueiyen 0:6bee2baac968 186
dikueiyen 0:6bee2baac968 187 for(uint8_t i = 0;i<3;i++){// 3 fillword ?
dikueiyen 0:6bee2baac968 188 FW[i+1] = spi_MTI.write(i);
dikueiyen 0:6bee2baac968 189 }
dikueiyen 0:6bee2baac968 190 }
dikueiyen 0:6bee2baac968 191
dikueiyen 0:6bee2baac968 192 //uint8_t ReadProtInfo(){
dikueiyen 0:6bee2baac968 193 // len = 2;
dikueiyen 0:6bee2baac968 194 // cs_MTI = 0;
dikueiyen 0:6bee2baac968 195 // SendOpcode(ProtInfo);//send opcode
dikueiyen 0:6bee2baac968 196 // for(int i = 0;i<len;i++){//read data
dikueiyen 0:6bee2baac968 197 // buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:6bee2baac968 198 // }
dikueiyen 0:6bee2baac968 199 // cs_MTI = 1;
dikueiyen 0:6bee2baac968 200 // if(FW[0]!=0xFA||FW[1]!=0xFF||FW[2]!=0xFF||FW[3]!=0xFF){
dikueiyen 0:6bee2baac968 201 // printf("Error!!\n");
dikueiyen 0:6bee2baac968 202 // }
dikueiyen 0:6bee2baac968 203 // return buffer[1];
dikueiyen 0:6bee2baac968 204 //}
dikueiyen 0:6bee2baac968 205
dikueiyen 0:6bee2baac968 206 void ConfigureProt(_Bool M,_Bool N,_Bool O,_Bool P)
dikueiyen 0:6bee2baac968 207 {
dikueiyen 0:6bee2baac968 208 // printf("ConfigureProt \r\n");
dikueiyen 0:6bee2baac968 209 uint8_t config = (M<<3) | (N<<2) | (O<<1) | (P<<0);
dikueiyen 0:6bee2baac968 210 cs_MTI = 0;
dikueiyen 0:6bee2baac968 211 SendOpcode(ConfigProt);
dikueiyen 0:6bee2baac968 212 spi_MTI.write(config);
dikueiyen 0:6bee2baac968 213 cs_MTI = 1;
dikueiyen 0:6bee2baac968 214 }
dikueiyen 0:6bee2baac968 215
dikueiyen 0:6bee2baac968 216 void PipeStatus(){
dikueiyen 0:6bee2baac968 217 // printf("PipeStatus \r\n");
dikueiyen 0:6bee2baac968 218 len = 4;
dikueiyen 0:6bee2baac968 219 cs_MTI = 0;
dikueiyen 0:6bee2baac968 220 SendOpcode(PipeStat);//send opcode
dikueiyen 0:6bee2baac968 221 for(int i = 0;i<len;i++){//read data
dikueiyen 0:6bee2baac968 222 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:6bee2baac968 223 }
dikueiyen 0:6bee2baac968 224 cs_MTI = 1;
dikueiyen 0:6bee2baac968 225 notificationSize = buffer[0] | (buffer[1]<<8);
dikueiyen 0:6bee2baac968 226 measurementSize = buffer[2] | (buffer[3]<<8);
dikueiyen 0:6bee2baac968 227 //printf("nSize:%d\r\n",notificationSize);
dikueiyen 0:6bee2baac968 228 //printf("mSize:%d\r\n",measurementSize);
dikueiyen 0:6bee2baac968 229 }
dikueiyen 0:6bee2baac968 230
dikueiyen 0:6bee2baac968 231 //void NotificationPipe(){
dikueiyen 0:6bee2baac968 232 // cs_MTI = 0;
dikueiyen 0:6bee2baac968 233 // SendOpcode(NotiPipe);//send opcode
dikueiyen 0:6bee2baac968 234 // for(int i = 0;i<notificationSize;i++){//read data
dikueiyen 0:6bee2baac968 235 // buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:6bee2baac968 236 // }
dikueiyen 0:6bee2baac968 237 // cs_MTI = 1;
dikueiyen 0:6bee2baac968 238 //}
dikueiyen 0:6bee2baac968 239
dikueiyen 0:6bee2baac968 240 void MeasurementPipe(){
dikueiyen 0:6bee2baac968 241 // printf("MeasurementPipe \r\n");
dikueiyen 0:6bee2baac968 242 cs_MTI = 0;
dikueiyen 0:6bee2baac968 243 SendOpcode(MeasPipe);//send opcode
dikueiyen 0:6bee2baac968 244 for(int i = 0;i<measurementSize;i++){//read data
dikueiyen 0:6bee2baac968 245 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:6bee2baac968 246 }
dikueiyen 0:6bee2baac968 247 cs_MTI = 1;
dikueiyen 0:6bee2baac968 248 }
dikueiyen 0:6bee2baac968 249
dikueiyen 0:6bee2baac968 250 void ControlPipe(){
dikueiyen 0:6bee2baac968 251 cs_MTI = 0;
dikueiyen 0:6bee2baac968 252 SendOpcode(Control);//send opcode
dikueiyen 0:6bee2baac968 253 for(int i = 0;i<ctrl_len;i++){//read data
dikueiyen 0:6bee2baac968 254 buffer[i] = spi_MTI.write(ctrlBuf[i]);
dikueiyen 0:6bee2baac968 255 }
dikueiyen 0:6bee2baac968 256 cs_MTI = 1;
dikueiyen 0:6bee2baac968 257 }
dikueiyen 0:6bee2baac968 258
dikueiyen 0:6bee2baac968 259 void ReadData(){
dikueiyen 0:6bee2baac968 260 // printf("ReadData \r\n");
dikueiyen 0:6bee2baac968 261 PipeStatus();
dikueiyen 0:6bee2baac968 262 wait_us(100);
dikueiyen 0:6bee2baac968 263 MeasurementPipe();
dikueiyen 0:6bee2baac968 264 int len1,len2,len3,data_bytes;
dikueiyen 0:6bee2baac968 265 // printf("Measurement FINISH \r\n");
dikueiyen 0:6bee2baac968 266 // printf("buffer[0] == %d \r\n",buffer[0]);
dikueiyen 0:6bee2baac968 267 if(buffer[0] == 0x36){
dikueiyen 0:6bee2baac968 268 // printf("buffer \r\n");
dikueiyen 0:6bee2baac968 269 if(buffer[2]== 0x20&&buffer[3]== 0x30){//Read Euler Angle
dikueiyen 0:6bee2baac968 270 len1 = buffer[4];
dikueiyen 0:6bee2baac968 271 data_bytes = len1/3;
dikueiyen 0:6bee2baac968 272 for(int j=0;j<3;j++){
dikueiyen 0:6bee2baac968 273 uint32_t temp = (buffer[5+j*data_bytes]<<24) | (buffer[6+j*data_bytes]<<16) | (buffer[7+j*data_bytes]<<8) | (buffer[8+j*data_bytes]);
dikueiyen 0:6bee2baac968 274 eul[j].data1 = temp;
dikueiyen 0:6bee2baac968 275 euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
dikueiyen 0:6bee2baac968 276 }
dikueiyen 0:6bee2baac968 277 }
dikueiyen 0:6bee2baac968 278 if(buffer[4+len1+1]== 0x40&&buffer[4+len1+2]== 0x20){
dikueiyen 0:6bee2baac968 279 len2 = buffer[4+len1+3];
dikueiyen 0:6bee2baac968 280 data_bytes = len2/3;
dikueiyen 0:6bee2baac968 281 for(int j=0;j<3;j++){
dikueiyen 0:6bee2baac968 282 uint32_t temp = (buffer[8+len1+j*data_bytes]<<24) | (buffer[9+len1+j*data_bytes]<<16) | (buffer[10+len1+j*data_bytes]<<8) | (buffer[11+len1+j*data_bytes]);
dikueiyen 0:6bee2baac968 283 acc[j].data1 = temp;
dikueiyen 0:6bee2baac968 284 accel[j] = lpf(acc[j].data2, accel[j], 13.0f);
dikueiyen 0:6bee2baac968 285 }
dikueiyen 0:6bee2baac968 286 }
dikueiyen 0:6bee2baac968 287 if(buffer[7+len1+len2+1]== 0x80&&buffer[7+len1+len2+2]==0x20){
dikueiyen 0:6bee2baac968 288 len3 = buffer[7+len1+len2+3];
dikueiyen 0:6bee2baac968 289 data_bytes = len3/3;
dikueiyen 0:6bee2baac968 290 for(int j=0;j<3;j++){
dikueiyen 0:6bee2baac968 291 uint32_t temp = (buffer[11+len1+len2+j*data_bytes]<<24) | (buffer[12+len1+len2+j*data_bytes]<<16) | (buffer[13+len1+len2+j*data_bytes]<<8) | (buffer[14+len1+len2+j*data_bytes]);
dikueiyen 0:6bee2baac968 292 gry[j].data1 = temp;
dikueiyen 0:6bee2baac968 293 omega[j] = lpf(gry[j].data2, omega[j], 13.0f);
dikueiyen 0:6bee2baac968 294 }
dikueiyen 0:6bee2baac968 295 }
dikueiyen 0:6bee2baac968 296
dikueiyen 0:6bee2baac968 297 }
dikueiyen 0:6bee2baac968 298 accel_[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
dikueiyen 0:6bee2baac968 299 accel_[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
dikueiyen 0:6bee2baac968 300 accel_[2] = accel[2];
dikueiyen 0:6bee2baac968 301 }
dikueiyen 0:6bee2baac968 302
dikueiyen 0:6bee2baac968 303 void MTi2_Init(){
dikueiyen 0:6bee2baac968 304 // printf("Init \r\n");
dikueiyen 0:6bee2baac968 305 cs_MTI = 1;///???
dikueiyen 0:6bee2baac968 306 ConfigureProt(1,0,0,0);//Configure DRDY
dikueiyen 0:6bee2baac968 307 }
dikueiyen 0:6bee2baac968 308
dikueiyen 0:6bee2baac968 309 float lpf(float input, float output_old, float frequency)
dikueiyen 0:6bee2baac968 310 {
dikueiyen 0:6bee2baac968 311 float output = 0;
dikueiyen 0:6bee2baac968 312 output = (output_old + frequency * dt * input) / (1 + frequency * dt);
dikueiyen 0:6bee2baac968 313 return output;
dikueiyen 0:6bee2baac968 314 }
dikueiyen 0:6bee2baac968 315
dikueiyen 0:6bee2baac968 316 //GSS
dikueiyen 0:6bee2baac968 317
dikueiyen 0:6bee2baac968 318 uint8_t readRegister(uint8_t addr) {
dikueiyen 0:6bee2baac968 319 wait_us(10); //tswr
dikueiyen 0:6bee2baac968 320
dikueiyen 0:6bee2baac968 321 cs = 0;
dikueiyen 0:6bee2baac968 322 addr = addr & 0x7F; //Set MSB to 0 to indicate read operation
dikueiyen 0:6bee2baac968 323
dikueiyen 0:6bee2baac968 324 spi.write(addr);
dikueiyen 0:6bee2baac968 325
dikueiyen 0:6bee2baac968 326 wait_us(35);
dikueiyen 0:6bee2baac968 327
dikueiyen 0:6bee2baac968 328 uint8_t data_read = spi.write(0U);
dikueiyen 0:6bee2baac968 329
dikueiyen 0:6bee2baac968 330 wait_us(1); //tsclk-ncs
dikueiyen 0:6bee2baac968 331 cs = 1;
dikueiyen 0:6bee2baac968 332 wait_us(20); //tsclk-ncs
dikueiyen 0:6bee2baac968 333 return data_read; //Returns 8-bit data from register
dikueiyen 0:6bee2baac968 334 }
dikueiyen 0:6bee2baac968 335
dikueiyen 0:6bee2baac968 336 //=========================================================================
dikueiyen 0:6bee2baac968 337 void writeRegister(uint8_t addr, uint8_t data) {
dikueiyen 0:6bee2baac968 338 cs = 0;
dikueiyen 0:6bee2baac968 339 addr = addr | 0x80; //Set MSB to 1 to indicate write operation
dikueiyen 0:6bee2baac968 340
dikueiyen 0:6bee2baac968 341 spi.write(addr);
dikueiyen 0:6bee2baac968 342
dikueiyen 0:6bee2baac968 343 spi.write(data);
dikueiyen 0:6bee2baac968 344
dikueiyen 0:6bee2baac968 345 wait_us(25); //tsclk-ncs
dikueiyen 0:6bee2baac968 346 cs = 1;
dikueiyen 0:6bee2baac968 347 wait_us(1); //tsclk-ncs
dikueiyen 0:6bee2baac968 348 }
dikueiyen 0:6bee2baac968 349
dikueiyen 0:6bee2baac968 350 //=========================================================================
dikueiyen 0:6bee2baac968 351 void initializeSensor(void) {
dikueiyen 0:6bee2baac968 352 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 353 writeRegister(0x55, 0x01);
dikueiyen 0:6bee2baac968 354 writeRegister(0x50, 0x07);
dikueiyen 0:6bee2baac968 355 writeRegister(0x7F, 0x0E);
dikueiyen 0:6bee2baac968 356 writeRegister(0x43, 0x10);
dikueiyen 0:6bee2baac968 357
dikueiyen 0:6bee2baac968 358 if (readRegister(0x67) & 0x40)
dikueiyen 0:6bee2baac968 359 writeRegister(0x48, 0x04);
dikueiyen 0:6bee2baac968 360 else
dikueiyen 0:6bee2baac968 361 writeRegister(0x48, 0x02);
dikueiyen 0:6bee2baac968 362
dikueiyen 0:6bee2baac968 363 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 364 writeRegister(0x51, 0x7B);
dikueiyen 0:6bee2baac968 365 writeRegister(0x50, 0x00);
dikueiyen 0:6bee2baac968 366 writeRegister(0x55, 0x00);
dikueiyen 0:6bee2baac968 367 writeRegister(0x7F, 0x0E);
dikueiyen 0:6bee2baac968 368
dikueiyen 0:6bee2baac968 369 if (readRegister(0x73) == 0x00) {
dikueiyen 0:6bee2baac968 370 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 371 writeRegister(0x61, 0xAD);
dikueiyen 0:6bee2baac968 372 writeRegister(0x51, 0x70);
dikueiyen 0:6bee2baac968 373 writeRegister(0x7F, 0x0E);
dikueiyen 0:6bee2baac968 374
dikueiyen 0:6bee2baac968 375 if (readRegister(0x70) <= 28)
dikueiyen 0:6bee2baac968 376 writeRegister(0x70, readRegister(0x70) + 14);
dikueiyen 0:6bee2baac968 377 else
dikueiyen 0:6bee2baac968 378 writeRegister(0x70, readRegister(0x70) + 11);
dikueiyen 0:6bee2baac968 379
dikueiyen 0:6bee2baac968 380 writeRegister(0x71, readRegister(0x71) * 45 / 100);
dikueiyen 0:6bee2baac968 381 }
dikueiyen 0:6bee2baac968 382
dikueiyen 0:6bee2baac968 383 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 384 writeRegister(0x61, 0xAD);
dikueiyen 0:6bee2baac968 385 writeRegister(0x7F, 0x03);
dikueiyen 0:6bee2baac968 386 writeRegister(0x40, 0x00);
dikueiyen 0:6bee2baac968 387 writeRegister(0x7F, 0x05);
dikueiyen 0:6bee2baac968 388 writeRegister(0x41, 0xB3);
dikueiyen 0:6bee2baac968 389 writeRegister(0x43, 0xF1);
dikueiyen 0:6bee2baac968 390 writeRegister(0x45, 0x14);
dikueiyen 0:6bee2baac968 391 writeRegister(0x5B, 0x32);
dikueiyen 0:6bee2baac968 392 writeRegister(0x5F, 0x34);
dikueiyen 0:6bee2baac968 393 writeRegister(0x7B, 0x08);
dikueiyen 0:6bee2baac968 394 writeRegister(0x7F, 0x06);
dikueiyen 0:6bee2baac968 395 writeRegister(0x44, 0x1B);
dikueiyen 0:6bee2baac968 396 writeRegister(0x40, 0xBF);
dikueiyen 0:6bee2baac968 397 writeRegister(0x4E, 0x3F);
dikueiyen 0:6bee2baac968 398 writeRegister(0x7F, 0x06);
dikueiyen 0:6bee2baac968 399 writeRegister(0x44, 0x1B);
dikueiyen 0:6bee2baac968 400 writeRegister(0x40, 0xBF);
dikueiyen 0:6bee2baac968 401 writeRegister(0x4E, 0x3F);
dikueiyen 0:6bee2baac968 402 writeRegister(0x7F, 0x08);
dikueiyen 0:6bee2baac968 403 writeRegister(0x65, 0x20);
dikueiyen 0:6bee2baac968 404 writeRegister(0x6A, 0x18);
dikueiyen 0:6bee2baac968 405 writeRegister(0x7F, 0x09);
dikueiyen 0:6bee2baac968 406 writeRegister(0x4F, 0xAF);
dikueiyen 0:6bee2baac968 407 writeRegister(0x5F, 0x40);
dikueiyen 0:6bee2baac968 408 writeRegister(0x48, 0x80);
dikueiyen 0:6bee2baac968 409 writeRegister(0x49, 0x80);
dikueiyen 0:6bee2baac968 410 writeRegister(0x57, 0x77);
dikueiyen 0:6bee2baac968 411 writeRegister(0x60, 0x78);
dikueiyen 0:6bee2baac968 412 writeRegister(0x61, 0x78);
dikueiyen 0:6bee2baac968 413 writeRegister(0x62, 0x08);
dikueiyen 0:6bee2baac968 414 writeRegister(0x63, 0x50);
dikueiyen 0:6bee2baac968 415 writeRegister(0x7F, 0x0A);
dikueiyen 0:6bee2baac968 416 writeRegister(0x45, 0x60);
dikueiyen 0:6bee2baac968 417 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 418 writeRegister(0x4D, 0x11);
dikueiyen 0:6bee2baac968 419 writeRegister(0x55, 0x80);
dikueiyen 0:6bee2baac968 420 writeRegister(0x74, 0x21);
dikueiyen 0:6bee2baac968 421 writeRegister(0x75, 0x1F);
dikueiyen 0:6bee2baac968 422 writeRegister(0x4A, 0x78);
dikueiyen 0:6bee2baac968 423 writeRegister(0x4B, 0x78);
dikueiyen 0:6bee2baac968 424 writeRegister(0x44, 0x08);
dikueiyen 0:6bee2baac968 425 writeRegister(0x45, 0x50);
dikueiyen 0:6bee2baac968 426 writeRegister(0x64, 0xFF);
dikueiyen 0:6bee2baac968 427 writeRegister(0x65, 0x1F);
dikueiyen 0:6bee2baac968 428 writeRegister(0x7F, 0x14);
dikueiyen 0:6bee2baac968 429 writeRegister(0x65, 0x67);
dikueiyen 0:6bee2baac968 430 writeRegister(0x66, 0x08);
dikueiyen 0:6bee2baac968 431 writeRegister(0x63, 0x70);
dikueiyen 0:6bee2baac968 432 writeRegister(0x7F, 0x15);
dikueiyen 0:6bee2baac968 433 writeRegister(0x48, 0x48);
dikueiyen 0:6bee2baac968 434 writeRegister(0x7F, 0x07);
dikueiyen 0:6bee2baac968 435 writeRegister(0x41, 0x0D);
dikueiyen 0:6bee2baac968 436 writeRegister(0x43, 0x14);
dikueiyen 0:6bee2baac968 437 writeRegister(0x4B, 0x0E);
dikueiyen 0:6bee2baac968 438 writeRegister(0x45, 0x0F);
dikueiyen 0:6bee2baac968 439 writeRegister(0x44, 0x42);
dikueiyen 0:6bee2baac968 440 writeRegister(0x4C, 0x80);
dikueiyen 0:6bee2baac968 441 writeRegister(0x7F, 0x10);
dikueiyen 0:6bee2baac968 442 writeRegister(0x5B, 0x02);
dikueiyen 0:6bee2baac968 443 writeRegister(0x7F, 0x07);
dikueiyen 0:6bee2baac968 444 writeRegister(0x40, 0x41);
dikueiyen 0:6bee2baac968 445 writeRegister(0x70, 0x00);
dikueiyen 0:6bee2baac968 446
dikueiyen 0:6bee2baac968 447 wait_ms(10);
dikueiyen 0:6bee2baac968 448
dikueiyen 0:6bee2baac968 449 writeRegister(0x32, 0x44);
dikueiyen 0:6bee2baac968 450 writeRegister(0x7F, 0x07);
dikueiyen 0:6bee2baac968 451 writeRegister(0x40, 0x40);
dikueiyen 0:6bee2baac968 452 writeRegister(0x7F, 0x06);
dikueiyen 0:6bee2baac968 453 writeRegister(0x62, 0xF0);
dikueiyen 0:6bee2baac968 454 writeRegister(0x63, 0x00);
dikueiyen 0:6bee2baac968 455 writeRegister(0x7F, 0x0D);
dikueiyen 0:6bee2baac968 456 writeRegister(0x48, 0xC0);
dikueiyen 0:6bee2baac968 457 writeRegister(0x6F, 0xD5);
dikueiyen 0:6bee2baac968 458 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 459 writeRegister(0x5B, 0xA0);
dikueiyen 0:6bee2baac968 460 writeRegister(0x4E, 0xA8);
dikueiyen 0:6bee2baac968 461 writeRegister(0x5A, 0x50);
dikueiyen 0:6bee2baac968 462 writeRegister(0x40, 0x80);
dikueiyen 0:6bee2baac968 463
dikueiyen 0:6bee2baac968 464 wait_ms(250);
dikueiyen 0:6bee2baac968 465
dikueiyen 0:6bee2baac968 466 writeRegister(0x7F, 0x14);
dikueiyen 0:6bee2baac968 467 writeRegister(0x6F, 0x1C);
dikueiyen 0:6bee2baac968 468 writeRegister(0x7F, 0x00);
dikueiyen 0:6bee2baac968 469
dikueiyen 0:6bee2baac968 470 }
dikueiyen 0:6bee2baac968 471 void grabData(void) {
dikueiyen 0:6bee2baac968 472 static int totalX = 0;
dikueiyen 0:6bee2baac968 473 static int totalY = 0;
dikueiyen 0:6bee2baac968 474 uint8_t check = 0;
dikueiyen 0:6bee2baac968 475 if(button_state == true){
dikueiyen 0:6bee2baac968 476 check = readRegister(0x02) & 0x80;
dikueiyen 0:6bee2baac968 477 if (check) {
dikueiyen 0:6bee2baac968 478 deltaX_low = readRegister(0x03); //Grabs data from the proper registers.
dikueiyen 0:6bee2baac968 479 deltaX_high = (readRegister(0x04) << 8) & 0xFF00; //Grabs data and shifts it to make space to be combined with lower bits.
dikueiyen 0:6bee2baac968 480 deltaY_low = readRegister(0x05);
dikueiyen 0:6bee2baac968 481 deltaY_high = (readRegister(0x06) << 8) & 0xFF00;
dikueiyen 0:6bee2baac968 482
dikueiyen 0:6bee2baac968 483 deltaY = deltaX_high | deltaX_low; //Combines the low and high bits.
dikueiyen 0:6bee2baac968 484 deltaX = deltaY_high | deltaY_low;
dikueiyen 0:6bee2baac968 485 totalX += deltaX;
dikueiyen 0:6bee2baac968 486 totalY += deltaY;
dikueiyen 0:6bee2baac968 487
dikueiyen 0:6bee2baac968 488 }
dikueiyen 0:6bee2baac968 489 GSS_X = totalX;
dikueiyen 0:6bee2baac968 490 GSS_Y = totalY;
dikueiyen 0:6bee2baac968 491 }
dikueiyen 0:6bee2baac968 492 }
dikueiyen 0:6bee2baac968 493
dikueiyen 0:6bee2baac968 494 //motor
dikueiyen 0:6bee2baac968 495 void ReadVelocity(){
dikueiyen 0:6bee2baac968 496 /*
dikueiyen 0:6bee2baac968 497 The velocity is calculated by follow :
dikueiyen 0:6bee2baac968 498 velocity = EncoderPosition /Encoder CPR (Counts per round) /gear ratio *2pi /dt
dikueiyen 0:6bee2baac968 499 unit : rad/sec
dikueiyen 0:6bee2baac968 500 */
dikueiyen 0:6bee2baac968 501
dikueiyen 0:6bee2baac968 502 EncoderPositionA = TIM2->CNT ;
dikueiyen 0:6bee2baac968 503 EncoderPositionB = TIM3->CNT ;
dikueiyen 0:6bee2baac968 504 TIM2->CNT = 0;
dikueiyen 0:6bee2baac968 505 TIM3->CNT = 0;
dikueiyen 0:6bee2baac968 506 // rad/s
dikueiyen 0:6bee2baac968 507 velocityA = EncoderPositionA /HALL_RESOLUTION /GEAR_RATIO /dt *60;
dikueiyen 0:6bee2baac968 508 velocityB = EncoderPositionB /HALL_RESOLUTION /GEAR_RATIO /dt *60;
dikueiyen 0:6bee2baac968 509 // RPM
dikueiyen 0:6bee2baac968 510 // *velocityA = EncoderPositionA /64.0 /56.0 /dt *60.0;
dikueiyen 0:6bee2baac968 511 // *velocityB = EncoderPositionB /64.0 /56.0 /dt *60.0;
dikueiyen 0:6bee2baac968 512 }
dikueiyen 0:6bee2baac968 513
dikueiyen 0:6bee2baac968 514
dikueiyen 0:6bee2baac968 515 void motor_drive(float voltA, float voltB){
dikueiyen 0:6bee2baac968 516 // Input voltage is in range -12.5V ~ 12.5V
dikueiyen 0:6bee2baac968 517 if(abs(voltA) <= minimum_volt){
dikueiyen 0:6bee2baac968 518 if(voltA > 0){ voltA = minimum_volt; }
dikueiyen 0:6bee2baac968 519 else{ voltA = -minimum_volt; }
dikueiyen 0:6bee2baac968 520 }
dikueiyen 0:6bee2baac968 521 if(abs(voltB) <= minimum_volt){
dikueiyen 0:6bee2baac968 522 if(voltB > 0){ voltB = minimum_volt; }
dikueiyen 0:6bee2baac968 523 else{ voltB = -minimum_volt; }
dikueiyen 0:6bee2baac968 524 }
dikueiyen 0:6bee2baac968 525
dikueiyen 0:6bee2baac968 526 // Convet volt to pwm
dikueiyen 0:6bee2baac968 527 uint16_t dutycycleA = (0.5f - 0.5f *voltA /INPUT_VOLTAGE) *uint16_t(TIM1->ARR);
dikueiyen 0:6bee2baac968 528 uint16_t dutycycleB = (0.5f - 0.5f *voltB /INPUT_VOLTAGE) *uint16_t(TIM1->ARR);
dikueiyen 0:6bee2baac968 529 TIM1->CCR1 = dutycycleA;
dikueiyen 0:6bee2baac968 530 TIM1->CCR2 = dutycycleB;
dikueiyen 0:6bee2baac968 531 }
dikueiyen 0:6bee2baac968 532
dikueiyen 0:6bee2baac968 533
dikueiyen 0:6bee2baac968 534 void control_speed(){
dikueiyen 0:6bee2baac968 535 float voltA;
dikueiyen 0:6bee2baac968 536 float voltB;
dikueiyen 0:6bee2baac968 537 // if receive 0 command than reset every thing
dikueiyen 0:6bee2baac968 538 if(VELOCITY_SPEED_A == 0 && VELOCITY_SPEED_B == 0)
dikueiyen 0:6bee2baac968 539 {
dikueiyen 0:6bee2baac968 540 velocityA = 0;
dikueiyen 0:6bee2baac968 541 velocityB = 0;
dikueiyen 0:6bee2baac968 542 last_voltA = 0;
dikueiyen 0:6bee2baac968 543 last_voltB = 0;
dikueiyen 0:6bee2baac968 544 errorA = 0;
dikueiyen 0:6bee2baac968 545 error_drA = 0;
dikueiyen 0:6bee2baac968 546 errorB = 0;
dikueiyen 0:6bee2baac968 547 error_drB = 0;
dikueiyen 0:6bee2baac968 548 }
dikueiyen 0:6bee2baac968 549 errorA = (VELOCITY_SPEED_A - velocityA);//(command from TX2 - read from odometry)
dikueiyen 0:6bee2baac968 550 voltA = last_voltA + 0.4f*errorA - 0.35f*error_drA;
dikueiyen 0:6bee2baac968 551 error_drA = errorA;
dikueiyen 0:6bee2baac968 552 last_voltA = voltA;
dikueiyen 0:6bee2baac968 553 if(abs(voltA) > INPUT_VOLTAGE){
dikueiyen 0:6bee2baac968 554 if(voltA > 0){voltA = INPUT_VOLTAGE;}
dikueiyen 0:6bee2baac968 555 else{voltA = -INPUT_VOLTAGE;}
dikueiyen 0:6bee2baac968 556 }
dikueiyen 0:6bee2baac968 557
dikueiyen 0:6bee2baac968 558 errorB = (VELOCITY_SPEED_B - velocityB);
dikueiyen 0:6bee2baac968 559 voltB = last_voltB + 0.4f*errorB - 0.35f*error_drB;
dikueiyen 0:6bee2baac968 560 error_drB = errorB;
dikueiyen 0:6bee2baac968 561 last_voltB = voltB;
dikueiyen 0:6bee2baac968 562 if(abs(voltB) > INPUT_VOLTAGE){
dikueiyen 0:6bee2baac968 563 if(voltB > 0){voltB = INPUT_VOLTAGE;}
dikueiyen 0:6bee2baac968 564 else{voltB = -INPUT_VOLTAGE;}
dikueiyen 0:6bee2baac968 565 }
dikueiyen 0:6bee2baac968 566
dikueiyen 0:6bee2baac968 567 motor_drive(voltA, voltB);
dikueiyen 0:6bee2baac968 568
dikueiyen 0:6bee2baac968 569 //printf("%.3f, %.3f, %.3f\r\n",error1, last_error, voltA);
dikueiyen 0:6bee2baac968 570 }
dikueiyen 0:6bee2baac968 571
dikueiyen 0:6bee2baac968 572
dikueiyen 0:6bee2baac968 573 void InitEncoder(void) {
dikueiyen 0:6bee2baac968 574 // Hardware Quadrature Encoder AB for Nucleo F446RE
dikueiyen 0:6bee2baac968 575 // Output on debug port to host PC @ 9600 baud
dikueiyen 0:6bee2baac968 576
dikueiyen 0:6bee2baac968 577 /* Connections
dikueiyen 0:6bee2baac968 578 PA_0 = Encoder1 A
dikueiyen 0:6bee2baac968 579 PA_1 = Encoder1 B
dikueiyen 0:6bee2baac968 580 PB_5 = Encoder2 A
dikueiyen 0:6bee2baac968 581 PB_4 = Encoder2 B
dikueiyen 0:6bee2baac968 582 */
dikueiyen 0:6bee2baac968 583
dikueiyen 0:6bee2baac968 584 // configure GPIO PA0, PA1, PB5 & PB4 as inputs for Encoder
dikueiyen 0:6bee2baac968 585 RCC->AHB1ENR |= 0x00000003; // Enable clock for GPIOA & GPIOB
dikueiyen 0:6bee2baac968 586
dikueiyen 0:6bee2baac968 587 GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; // PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */
dikueiyen 0:6bee2baac968 588 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_0 | GPIO_PUPDR_PUPDR1_0 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */
dikueiyen 0:6bee2baac968 589 GPIOA->AFR[0] |= 0x00000011 ; // AF1 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
dikueiyen 0:6bee2baac968 590 GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
dikueiyen 0:6bee2baac968 591
dikueiyen 0:6bee2baac968 592
dikueiyen 0:6bee2baac968 593 GPIOB->MODER |= GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 ; // PB5 & PB4 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */
dikueiyen 0:6bee2baac968 594 GPIOB->PUPDR |= GPIO_PUPDR_PUPDR4_0 | GPIO_PUPDR_PUPDR5_0 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */
dikueiyen 0:6bee2baac968 595 GPIOB->AFR[0] |= 0x00220000 ; // AF2 for PB5 & PB4 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
dikueiyen 0:6bee2baac968 596 GPIOB->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
dikueiyen 0:6bee2baac968 597
dikueiyen 0:6bee2baac968 598 // configure TIM2 & TIM3 as Encoder input
dikueiyen 0:6bee2baac968 599 RCC->APB1ENR |= 0x00000003; // Enable clock for TIM2 & TIM3
dikueiyen 0:6bee2baac968 600
dikueiyen 0:6bee2baac968 601 TIM2->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1
dikueiyen 0:6bee2baac968 602 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
dikueiyen 0:6bee2baac968 603 TIM2->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
dikueiyen 0:6bee2baac968 604 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
dikueiyen 0:6bee2baac968 605 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
dikueiyen 0:6bee2baac968 606 TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
dikueiyen 0:6bee2baac968 607 TIM2->ARR = 0xffffffff; // reload at 0xfffffff < TIM auto-reload register
dikueiyen 0:6bee2baac968 608
dikueiyen 0:6bee2baac968 609 TIM2->CNT = 0x0000; //reset the counter before we use it
dikueiyen 0:6bee2baac968 610
dikueiyen 0:6bee2baac968 611 TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1
dikueiyen 0:6bee2baac968 612 TIM3->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
dikueiyen 0:6bee2baac968 613 TIM3->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
dikueiyen 0:6bee2baac968 614 TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
dikueiyen 0:6bee2baac968 615 TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
dikueiyen 0:6bee2baac968 616 TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
dikueiyen 0:6bee2baac968 617 TIM3->ARR = 0xffffffff; // reload at 0xfffffff < TIM auto-reload register
dikueiyen 0:6bee2baac968 618
dikueiyen 0:6bee2baac968 619 TIM3->CNT = 0x0000; //reset the counter before we use it
dikueiyen 0:6bee2baac968 620 }
dikueiyen 0:6bee2baac968 621
dikueiyen 0:6bee2baac968 622
dikueiyen 0:6bee2baac968 623 void InitMotor(float pwm_frequency){
dikueiyen 0:6bee2baac968 624 uint16_t reload = 90000000 /int(pwm_frequency * 1000) - 1;
dikueiyen 0:6bee2baac968 625 uint16_t stop = 90000000 /int(pwm_frequency * 1000) /2 - 1;
dikueiyen 0:6bee2baac968 626
dikueiyen 0:6bee2baac968 627 TIM1->CR1 &= (~0x0001); // Set counter disable in Control Register 1 at initial
dikueiyen 0:6bee2baac968 628 TIM1->PSC = 1U; // Prescaler system clock (1 + PSC) for Timer 1
dikueiyen 0:6bee2baac968 629 TIM1->ARR = reload; // Set auto-reload, the pwm freq is (system clk /(1+PSC) /ARR)
dikueiyen 0:6bee2baac968 630 TIM1->CCMR1 |= 0x0808; // Not necessary
dikueiyen 0:6bee2baac968 631 TIM1->CCER |= 0x0055; // Enable complementary mode for channel 1, channel 2
dikueiyen 0:6bee2baac968 632 TIM1->BDTR |= 0x0C00; // Set off-state selection
dikueiyen 0:6bee2baac968 633 TIM1->EGR = 0x0001; // Update generation
dikueiyen 0:6bee2baac968 634 TIM1->CR1 |= 0x0001; // Counter enable
dikueiyen 0:6bee2baac968 635 /*
dikueiyen 0:6bee2baac968 636 pc.printf("CR1 : %d\r",uint16_t(TIM1->CR1));
dikueiyen 0:6bee2baac968 637 pc.printf("PSC : %d\r",uint16_t(TIM1->PSC));
dikueiyen 0:6bee2baac968 638 pc.printf("ARR : %d\r",uint16_t(TIM1->ARR));
dikueiyen 0:6bee2baac968 639 pc.printf("CCMR1 : %x\r",TIM1->CCMR1);
dikueiyen 0:6bee2baac968 640 pc.printf("CCER : %x\r",TIM1->CCER);
dikueiyen 0:6bee2baac968 641 pc.printf("BDTR : %x\r",TIM1->BDTR);
dikueiyen 0:6bee2baac968 642 pc.printf("EGR : %x\r",TIM1->EGR);
dikueiyen 0:6bee2baac968 643 pc.printf("stop : %d\r",stop);
dikueiyen 0:6bee2baac968 644 */
dikueiyen 0:6bee2baac968 645 TIM1->CCR1 = stop;
dikueiyen 0:6bee2baac968 646 TIM1->CCR2 = stop;
dikueiyen 0:6bee2baac968 647
dikueiyen 0:6bee2baac968 648 // bool cc1ne_bit = (TIM1->CCER >> 2) & 0x0001;
dikueiyen 0:6bee2baac968 649 // pc.printf("CC1NE bit : %d\r",cc1ne_bit);
dikueiyen 0:6bee2baac968 650 }
dikueiyen 0:6bee2baac968 651
dikueiyen 0:6bee2baac968 652
dikueiyen 0:6bee2baac968 653 void init_UART()
dikueiyen 0:6bee2baac968 654 {
dikueiyen 1:fdfd9a35acc4 655 pc.baud(230400); // baud rate閮剔9600
dikueiyen 0:6bee2baac968 656 pc.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
dikueiyen 0:6bee2baac968 657 }
dikueiyen 0:6bee2baac968 658
dikueiyen 0:6bee2baac968 659
dikueiyen 0:6bee2baac968 660 void RX_ITR()
dikueiyen 0:6bee2baac968 661 {
dikueiyen 0:6bee2baac968 662 while(pc.readable()) {
dikueiyen 0:6bee2baac968 663 char uart_read;
dikueiyen 0:6bee2baac968 664 uart_read = pc.getc();
dikueiyen 0:6bee2baac968 665 if(uart_read == 115) {
dikueiyen 0:6bee2baac968 666 RX_flag2 = 1;
dikueiyen 0:6bee2baac968 667 readcount = 0;
dikueiyen 0:6bee2baac968 668 getData[5] = 0;
dikueiyen 0:6bee2baac968 669 }
dikueiyen 0:6bee2baac968 670 if(RX_flag2 == 1) {
dikueiyen 0:6bee2baac968 671 getData[readcount] = uart_read;
dikueiyen 0:6bee2baac968 672 readcount++;
dikueiyen 0:6bee2baac968 673 if(readcount >= 6 & getData[5] == 101) {
dikueiyen 0:6bee2baac968 674 readcount = 0;
dikueiyen 0:6bee2baac968 675 RX_flag2 = 0;
dikueiyen 0:6bee2baac968 676 ///code for decoding///
dikueiyen 0:6bee2baac968 677 data_received[0] = (getData[2] << 8) | getData[1];
dikueiyen 0:6bee2baac968 678 data_received[1] = (getData[4] << 8) | getData[3];
dikueiyen 0:6bee2baac968 679 // VELOCITY_SPEED_A = data_received[0]/100;
dikueiyen 0:6bee2baac968 680 // VELOCITY_SPEED_B = data_received[1]/100;
dikueiyen 0:6bee2baac968 681 VELOCITY_SPEED_A = (float)data_received[0]/100.0f;
dikueiyen 0:6bee2baac968 682 VELOCITY_SPEED_B = (float)data_received[1]/100.0f;
dikueiyen 0:6bee2baac968 683 ///////////////////////
dikueiyen 0:6bee2baac968 684 }
dikueiyen 0:6bee2baac968 685 }
dikueiyen 0:6bee2baac968 686 }
dikueiyen 0:6bee2baac968 687 }