dikuei yen / Mbed 2 deprecated ground_speed_sensor

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:b24eb0b62fd0, 2022-05-02 (annotated)

Committer:
dikueiyen
Date:
Mon May 02 10:21:36 2022 +0000
Revision:
1:b24eb0b62fd0
Parent:
0:684b50f013f7 
20220502;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jkjk010695 0:684b50f013f7 1 #include "mbed.h"
jkjk010695 0:684b50f013f7 2 #include <math.h>
jkjk010695 0:684b50f013f7 3 #include <stdlib.h>
dikueiyen 1:b24eb0b62fd0 4 #include "PMW3901.h"
jkjk010695 0:684b50f013f7 5
jkjk010695 0:684b50f013f7 6 Serial pc(USBTX,USBRX);
jkjk010695 0:684b50f013f7 7 InterruptIn mybutton(USER_BUTTON);
jkjk010695 0:684b50f013f7 8 Ticker main_function; //interrupt
dikueiyen 1:b24eb0b62fd0 9
jkjk010695 0:684b50f013f7 10 DigitalOut led1(LED1);
dikueiyen 1:b24eb0b62fd0 11 //SPI speed_sensor(PC_12,PC_11,PC_10);
dikueiyen 1:b24eb0b62fd0 12 SPI spi(PC_12,PC_11,PC_10);
dikueiyen 1:b24eb0b62fd0 13 //DigitalOut CS(PA_4);
dikueiyen 1:b24eb0b62fd0 14 DigitalOut cs(PA_4);
jkjk010695 0:684b50f013f7 15
jkjk010695 0:684b50f013f7 16 float dt = 0.01; // sec
jkjk010695 0:684b50f013f7 17 float command = 0;
jkjk010695 0:684b50f013f7 18 bool button_state = false;
dikueiyen 1:b24eb0b62fd0 19
dikueiyen 1:b24eb0b62fd0 20 /*void InitEncoder(void);*/
dikueiyen 1:b24eb0b62fd0 21
dikueiyen 1:b24eb0b62fd0 22 void init_SPI();
dikueiyen 1:b24eb0b62fd0 23
dikueiyen 1:b24eb0b62fd0 24 void start(void);
dikueiyen 1:b24eb0b62fd0 25 void grabData(void);
dikueiyen 1:b24eb0b62fd0 26 //void printData(void);
dikueiyen 1:b24eb0b62fd0 27 void initializeSensor(void);
dikueiyen 1:b24eb0b62fd0 28 void writeRegister(uint8_t addr, uint8_t data);
dikueiyen 1:b24eb0b62fd0 29 uint8_t readRegister(uint8_t addr);
dikueiyen 1:b24eb0b62fd0 30 void delayus(uint32_t us);
jkjk010695 0:684b50f013f7 31
jkjk010695 0:684b50f013f7 32 void step_command();
jkjk010695 0:684b50f013f7 33
dikueiyen 1:b24eb0b62fd0 34 //void RX_ITR();
dikueiyen 1:b24eb0b62fd0 35 //void init_UART();
jkjk010695 0:684b50f013f7 36
jkjk010695 0:684b50f013f7 37 int main() {
jkjk010695 0:684b50f013f7 38 pc.baud(115200);
dikueiyen 1:b24eb0b62fd0 39 init_SPI();
dikueiyen 1:b24eb0b62fd0 40 // InitEncoder(); //don't care
dikueiyen 1:b24eb0b62fd0 41 //InitMotor(PWM_FREQUENCY); // Set pwm period to 1ms.
dikueiyen 1:b24eb0b62fd0 42 //init_UART();
jkjk010695 0:684b50f013f7 43 mybutton.fall(&step_command);
dikueiyen 1:b24eb0b62fd0 44 initializeSensor();
dikueiyen 1:b24eb0b62fd0 45 //main_function.attach_us(&position_control, dt*1000000);
dikueiyen 1:b24eb0b62fd0 46 main_function.attach_us(&start, dt*1000000);
jkjk010695 0:684b50f013f7 47
jkjk010695 0:684b50f013f7 48 while(1){}
jkjk010695 0:684b50f013f7 49 }
jkjk010695 0:684b50f013f7 50
dikueiyen 1:b24eb0b62fd0 51 void start(){
dikueiyen 1:b24eb0b62fd0 52 cs = 0;
dikueiyen 1:b24eb0b62fd0 53 grabData();
dikueiyen 1:b24eb0b62fd0 54 //printData();
dikueiyen 1:b24eb0b62fd0 55 cs = 1;
jkjk010695 0:684b50f013f7 56 }
jkjk010695 0:684b50f013f7 57
dikueiyen 1:b24eb0b62fd0 58 void init_SPI(){
dikueiyen 1:b24eb0b62fd0 59 //CS.output = 1;
dikueiyen 1:b24eb0b62fd0 60 cs = 1;
dikueiyen 1:b24eb0b62fd0 61 //speed_sensor.format(8, 3);
dikueiyen 1:b24eb0b62fd0 62 //speed_sensor.frequency(1000000);
dikueiyen 1:b24eb0b62fd0 63 spi.format(8, 3);
dikueiyen 1:b24eb0b62fd0 64 spi.frequency(1000000);
dikueiyen 1:b24eb0b62fd0 65 }
jkjk010695 0:684b50f013f7 66
jkjk010695 0:684b50f013f7 67 void step_command(){
jkjk010695 0:684b50f013f7 68 led1 = !led1;
jkjk010695 0:684b50f013f7 69 button_state = !button_state;
jkjk010695 0:684b50f013f7 70 }
jkjk010695 0:684b50f013f7 71
dikueiyen 1:b24eb0b62fd0 72 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
dikueiyen 1:b24eb0b62fd0 73 //=========================================================================
dikueiyen 1:b24eb0b62fd0 74 //Functions definitions
dikueiyen 1:b24eb0b62fd0 75 //=========================================================================
jkjk010695 0:684b50f013f7 76
dikueiyen 1:b24eb0b62fd0 77 uint8_t readRegister(uint8_t addr) {
dikueiyen 1:b24eb0b62fd0 78 wait_us(10); //tswr
dikueiyen 1:b24eb0b62fd0 79 //LL_GPIO_ResetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
dikueiyen 1:b24eb0b62fd0 80 cs = 0;
dikueiyen 1:b24eb0b62fd0 81 addr = addr & 0x7F; //Set MSB to 0 to indicate read operation
jkjk010695 0:684b50f013f7 82
dikueiyen 1:b24eb0b62fd0 83 // LL_SPI_TransmitData16(SPI3, addr);
dikueiyen 1:b24eb0b62fd0 84 // while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) { //Stuck if not finished
dikueiyen 1:b24eb0b62fd0 85 // }
dikueiyen 1:b24eb0b62fd0 86 // LL_SPI_ReceiveData16(SPI3); //Just to clear RXNE
dikueiyen 1:b24eb0b62fd0 87 spi.write(addr);
dikueiyen 1:b24eb0b62fd0 88
dikueiyen 1:b24eb0b62fd0 89 wait_us(35);
dikueiyen 1:b24eb0b62fd0 90
dikueiyen 1:b24eb0b62fd0 91 // LL_SPI_TransmitData16(SPI3, 0U);
dikueiyen 1:b24eb0b62fd0 92 // while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) { //Stuck if not finished
dikueiyen 1:b24eb0b62fd0 93 // }
dikueiyen 1:b24eb0b62fd0 94 // uint8_t data_read = LL_SPI_ReceiveData16(SPI3);
dikueiyen 1:b24eb0b62fd0 95 uint8_t data_read = spi.write(0U);
dikueiyen 1:b24eb0b62fd0 96
dikueiyen 1:b24eb0b62fd0 97 wait_us(1); //tsclk-ncs
dikueiyen 1:b24eb0b62fd0 98 //LL_GPIO_SetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
dikueiyen 1:b24eb0b62fd0 99 cs = 1;
dikueiyen 1:b24eb0b62fd0 100 wait_us(20); //tsclk-ncs
dikueiyen 1:b24eb0b62fd0 101 return data_read; //Returns 8-bit data from register
jkjk010695 0:684b50f013f7 102 }
jkjk010695 0:684b50f013f7 103
dikueiyen 1:b24eb0b62fd0 104 //=========================================================================
dikueiyen 1:b24eb0b62fd0 105 void writeRegister(uint8_t addr, uint8_t data) {
dikueiyen 1:b24eb0b62fd0 106 //LL_GPIO_ResetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
dikueiyen 1:b24eb0b62fd0 107 cs = 0;
dikueiyen 1:b24eb0b62fd0 108 addr = addr | 0x80; //Set MSB to 1 to indicate write operation
jkjk010695 0:684b50f013f7 109
dikueiyen 1:b24eb0b62fd0 110 // LL_SPI_TransmitData16(SPI3, addr);
dikueiyen 1:b24eb0b62fd0 111 // while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) { //Stuck if not finished
dikueiyen 1:b24eb0b62fd0 112 // }
dikueiyen 1:b24eb0b62fd0 113 // LL_SPI_ReceiveData16(SPI3); //Just to clear RXNE
dikueiyen 1:b24eb0b62fd0 114 spi.write(addr);
jkjk010695 0:684b50f013f7 115
dikueiyen 1:b24eb0b62fd0 116 // LL_SPI_TransmitData16(SPI3, data);
dikueiyen 1:b24eb0b62fd0 117 // while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) { //Stuck if not finished
dikueiyen 1:b24eb0b62fd0 118 // }
dikueiyen 1:b24eb0b62fd0 119 // LL_SPI_ReceiveData16(SPI3); //Just to clear RXNE
dikueiyen 1:b24eb0b62fd0 120 spi.write(data);
jkjk010695 0:684b50f013f7 121
dikueiyen 1:b24eb0b62fd0 122 wait_us(25); //tsclk-ncs
dikueiyen 1:b24eb0b62fd0 123
dikueiyen 1:b24eb0b62fd0 124 //LL_GPIO_SetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
dikueiyen 1:b24eb0b62fd0 125 cs = 1;
dikueiyen 1:b24eb0b62fd0 126 wait_us(1); //tsclk-ncs
jkjk010695 0:684b50f013f7 127 }
jkjk010695 0:684b50f013f7 128
dikueiyen 1:b24eb0b62fd0 129 //=========================================================================
dikueiyen 1:b24eb0b62fd0 130 void initializeSensor(void) {
dikueiyen 1:b24eb0b62fd0 131 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 132 writeRegister(0x55, 0x01);
dikueiyen 1:b24eb0b62fd0 133 writeRegister(0x50, 0x07);
dikueiyen 1:b24eb0b62fd0 134 writeRegister(0x7F, 0x0E);
dikueiyen 1:b24eb0b62fd0 135 writeRegister(0x43, 0x10);
dikueiyen 1:b24eb0b62fd0 136
dikueiyen 1:b24eb0b62fd0 137 if (readRegister(0x67) & 0x40)
dikueiyen 1:b24eb0b62fd0 138 writeRegister(0x48, 0x04);
dikueiyen 1:b24eb0b62fd0 139 else
dikueiyen 1:b24eb0b62fd0 140 writeRegister(0x48, 0x02);
dikueiyen 1:b24eb0b62fd0 141
dikueiyen 1:b24eb0b62fd0 142 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 143 writeRegister(0x51, 0x7B);
dikueiyen 1:b24eb0b62fd0 144 writeRegister(0x50, 0x00);
dikueiyen 1:b24eb0b62fd0 145 writeRegister(0x55, 0x00);
dikueiyen 1:b24eb0b62fd0 146 writeRegister(0x7F, 0x0E);
dikueiyen 1:b24eb0b62fd0 147
dikueiyen 1:b24eb0b62fd0 148 if (readRegister(0x73) == 0x00) {
dikueiyen 1:b24eb0b62fd0 149 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 150 writeRegister(0x61, 0xAD);
dikueiyen 1:b24eb0b62fd0 151 writeRegister(0x51, 0x70);
dikueiyen 1:b24eb0b62fd0 152 writeRegister(0x7F, 0x0E);
dikueiyen 1:b24eb0b62fd0 153
dikueiyen 1:b24eb0b62fd0 154 if (readRegister(0x70) <= 28)
dikueiyen 1:b24eb0b62fd0 155 writeRegister(0x70, readRegister(0x70) + 14);
dikueiyen 1:b24eb0b62fd0 156 else
dikueiyen 1:b24eb0b62fd0 157 writeRegister(0x70, readRegister(0x70) + 11);
dikueiyen 1:b24eb0b62fd0 158
dikueiyen 1:b24eb0b62fd0 159 writeRegister(0x71, readRegister(0x71) * 45 / 100);
jkjk010695 0:684b50f013f7 160 }
dikueiyen 1:b24eb0b62fd0 161
dikueiyen 1:b24eb0b62fd0 162 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 163 writeRegister(0x61, 0xAD);
dikueiyen 1:b24eb0b62fd0 164 writeRegister(0x7F, 0x03);
dikueiyen 1:b24eb0b62fd0 165 writeRegister(0x40, 0x00);
dikueiyen 1:b24eb0b62fd0 166 writeRegister(0x7F, 0x05);
dikueiyen 1:b24eb0b62fd0 167 writeRegister(0x41, 0xB3);
dikueiyen 1:b24eb0b62fd0 168 writeRegister(0x43, 0xF1);
dikueiyen 1:b24eb0b62fd0 169 writeRegister(0x45, 0x14);
dikueiyen 1:b24eb0b62fd0 170 writeRegister(0x5B, 0x32);
dikueiyen 1:b24eb0b62fd0 171 writeRegister(0x5F, 0x34);
dikueiyen 1:b24eb0b62fd0 172 writeRegister(0x7B, 0x08);
dikueiyen 1:b24eb0b62fd0 173 writeRegister(0x7F, 0x06);
dikueiyen 1:b24eb0b62fd0 174 writeRegister(0x44, 0x1B);
dikueiyen 1:b24eb0b62fd0 175 writeRegister(0x40, 0xBF);
dikueiyen 1:b24eb0b62fd0 176 writeRegister(0x4E, 0x3F);
dikueiyen 1:b24eb0b62fd0 177 writeRegister(0x7F, 0x06);
dikueiyen 1:b24eb0b62fd0 178 writeRegister(0x44, 0x1B);
dikueiyen 1:b24eb0b62fd0 179 writeRegister(0x40, 0xBF);
dikueiyen 1:b24eb0b62fd0 180 writeRegister(0x4E, 0x3F);
dikueiyen 1:b24eb0b62fd0 181 writeRegister(0x7F, 0x08);
dikueiyen 1:b24eb0b62fd0 182 writeRegister(0x65, 0x20);
dikueiyen 1:b24eb0b62fd0 183 writeRegister(0x6A, 0x18);
dikueiyen 1:b24eb0b62fd0 184 writeRegister(0x7F, 0x09);
dikueiyen 1:b24eb0b62fd0 185 writeRegister(0x4F, 0xAF);
dikueiyen 1:b24eb0b62fd0 186 writeRegister(0x5F, 0x40);
dikueiyen 1:b24eb0b62fd0 187 writeRegister(0x48, 0x80);
dikueiyen 1:b24eb0b62fd0 188 writeRegister(0x49, 0x80);
dikueiyen 1:b24eb0b62fd0 189 writeRegister(0x57, 0x77);
dikueiyen 1:b24eb0b62fd0 190 writeRegister(0x60, 0x78);
dikueiyen 1:b24eb0b62fd0 191 writeRegister(0x61, 0x78);
dikueiyen 1:b24eb0b62fd0 192 writeRegister(0x62, 0x08);
dikueiyen 1:b24eb0b62fd0 193 writeRegister(0x63, 0x50);
dikueiyen 1:b24eb0b62fd0 194 writeRegister(0x7F, 0x0A);
dikueiyen 1:b24eb0b62fd0 195 writeRegister(0x45, 0x60);
dikueiyen 1:b24eb0b62fd0 196 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 197 writeRegister(0x4D, 0x11);
dikueiyen 1:b24eb0b62fd0 198 writeRegister(0x55, 0x80);
dikueiyen 1:b24eb0b62fd0 199 writeRegister(0x74, 0x21);
dikueiyen 1:b24eb0b62fd0 200 writeRegister(0x75, 0x1F);
dikueiyen 1:b24eb0b62fd0 201 writeRegister(0x4A, 0x78);
dikueiyen 1:b24eb0b62fd0 202 writeRegister(0x4B, 0x78);
dikueiyen 1:b24eb0b62fd0 203 writeRegister(0x44, 0x08);
dikueiyen 1:b24eb0b62fd0 204 writeRegister(0x45, 0x50);
dikueiyen 1:b24eb0b62fd0 205 writeRegister(0x64, 0xFF);
dikueiyen 1:b24eb0b62fd0 206 writeRegister(0x65, 0x1F);
dikueiyen 1:b24eb0b62fd0 207 writeRegister(0x7F, 0x14);
dikueiyen 1:b24eb0b62fd0 208 writeRegister(0x65, 0x67);
dikueiyen 1:b24eb0b62fd0 209 writeRegister(0x66, 0x08);
dikueiyen 1:b24eb0b62fd0 210 writeRegister(0x63, 0x70);
dikueiyen 1:b24eb0b62fd0 211 writeRegister(0x7F, 0x15);
dikueiyen 1:b24eb0b62fd0 212 writeRegister(0x48, 0x48);
dikueiyen 1:b24eb0b62fd0 213 writeRegister(0x7F, 0x07);
dikueiyen 1:b24eb0b62fd0 214 writeRegister(0x41, 0x0D);
dikueiyen 1:b24eb0b62fd0 215 writeRegister(0x43, 0x14);
dikueiyen 1:b24eb0b62fd0 216 writeRegister(0x4B, 0x0E);
dikueiyen 1:b24eb0b62fd0 217 writeRegister(0x45, 0x0F);
dikueiyen 1:b24eb0b62fd0 218 writeRegister(0x44, 0x42);
dikueiyen 1:b24eb0b62fd0 219 writeRegister(0x4C, 0x80);
dikueiyen 1:b24eb0b62fd0 220 writeRegister(0x7F, 0x10);
dikueiyen 1:b24eb0b62fd0 221 writeRegister(0x5B, 0x02);
dikueiyen 1:b24eb0b62fd0 222 writeRegister(0x7F, 0x07);
dikueiyen 1:b24eb0b62fd0 223 writeRegister(0x40, 0x41);
dikueiyen 1:b24eb0b62fd0 224 writeRegister(0x70, 0x00);
dikueiyen 1:b24eb0b62fd0 225
dikueiyen 1:b24eb0b62fd0 226 wait_ms(10);
dikueiyen 1:b24eb0b62fd0 227
dikueiyen 1:b24eb0b62fd0 228 writeRegister(0x32, 0x44);
dikueiyen 1:b24eb0b62fd0 229 writeRegister(0x7F, 0x07);
dikueiyen 1:b24eb0b62fd0 230 writeRegister(0x40, 0x40);
dikueiyen 1:b24eb0b62fd0 231 writeRegister(0x7F, 0x06);
dikueiyen 1:b24eb0b62fd0 232 writeRegister(0x62, 0xF0);
dikueiyen 1:b24eb0b62fd0 233 writeRegister(0x63, 0x00);
dikueiyen 1:b24eb0b62fd0 234 writeRegister(0x7F, 0x0D);
dikueiyen 1:b24eb0b62fd0 235 writeRegister(0x48, 0xC0);
dikueiyen 1:b24eb0b62fd0 236 writeRegister(0x6F, 0xD5);
dikueiyen 1:b24eb0b62fd0 237 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 238 writeRegister(0x5B, 0xA0);
dikueiyen 1:b24eb0b62fd0 239 writeRegister(0x4E, 0xA8);
dikueiyen 1:b24eb0b62fd0 240 writeRegister(0x5A, 0x50);
dikueiyen 1:b24eb0b62fd0 241 writeRegister(0x40, 0x80);
dikueiyen 1:b24eb0b62fd0 242
dikueiyen 1:b24eb0b62fd0 243 wait_ms(250);
dikueiyen 1:b24eb0b62fd0 244
dikueiyen 1:b24eb0b62fd0 245 writeRegister(0x7F, 0x14);
dikueiyen 1:b24eb0b62fd0 246 writeRegister(0x6F, 0x1C);
dikueiyen 1:b24eb0b62fd0 247 writeRegister(0x7F, 0x00);
dikueiyen 1:b24eb0b62fd0 248
jkjk010695 0:684b50f013f7 249 }
jkjk010695 0:684b50f013f7 250
dikueiyen 1:b24eb0b62fd0 251 //=========================================================================
dikueiyen 1:b24eb0b62fd0 252 void grabData(void) {
dikueiyen 1:b24eb0b62fd0 253 static int totalX = 0;
dikueiyen 1:b24eb0b62fd0 254 static int totalY = 0;
dikueiyen 1:b24eb0b62fd0 255 uint8_t check = 0;
dikueiyen 1:b24eb0b62fd0 256 if(button_state == true){
dikueiyen 1:b24eb0b62fd0 257 check = readRegister(0x02) & 0x80;
dikueiyen 1:b24eb0b62fd0 258 if (check) {
dikueiyen 1:b24eb0b62fd0 259 deltaX_low = readRegister(0x03); //Grabs data from the proper registers.
dikueiyen 1:b24eb0b62fd0 260 deltaX_high = (readRegister(0x04) << 8) & 0xFF00; //Grabs data and shifts it to make space to be combined with lower bits.
dikueiyen 1:b24eb0b62fd0 261 deltaY_low = readRegister(0x05);
dikueiyen 1:b24eb0b62fd0 262 deltaY_high = (readRegister(0x06) << 8) & 0xFF00;
jkjk010695 0:684b50f013f7 263
dikueiyen 1:b24eb0b62fd0 264 deltaY = deltaX_high | deltaX_low; //Combines the low and high bits.
dikueiyen 1:b24eb0b62fd0 265 deltaX = deltaY_high | deltaY_low;
dikueiyen 1:b24eb0b62fd0 266 // pc.printf("deltaX: %d\t\t\tdeltaY: %d\n\r", deltaX, deltaY); //Prints each individual count of deltaX and deltaY.
dikueiyen 1:b24eb0b62fd0 267 //pc.printf("X-axis Counts: %d\t\tY-axis Counts: %d\n\r", totalX, totalY); //Prints the total movement made during runtime.
dikueiyen 1:b24eb0b62fd0 268 totalX += deltaX;
dikueiyen 1:b24eb0b62fd0 269 totalY += deltaY;
jkjk010695 0:684b50f013f7 270
dikueiyen 1:b24eb0b62fd0 271 }
jkjk010695 0:684b50f013f7 272
dikueiyen 1:b24eb0b62fd0 273 printf("%d,%d\n\r", totalX, totalY);
jkjk010695 0:684b50f013f7 274
jkjk010695 0:684b50f013f7 275 }
jkjk010695 0:684b50f013f7 276 }