Diff: main.cpp

Revision:

1:b24eb0b62fd0

Parent:

0:684b50f013f7

--- a/main.cpp	Mon Apr 27 08:33:33 2020 +0000
+++ b/main.cpp	Mon May 02 10:21:36 2022 +0000
@@ -1,312 +1,276 @@
 #include "mbed.h"
 #include <math.h>
 #include <stdlib.h>
-
-#define pi 3.14159265358979323846f
-#define maximum_volt 12.0f
-#define minimum_volt 0.45f // Need to test for different loads.
-
-#define INPUT_VOLTAGE       12.5f 
-#define PWM_FREQUENCY       50.0f   // 20kHz
-#define PWM_STOP            0.5f    //the pwm dutycycle value is from 0~1 and 0.5 can let motor stop
-
-#define FRICTION_VOLTAGE    0.45f
-#define HALL_RESOLUTION     64.0f
-#define GEAR_RATIO          56.0f      
-#define VELOCITY_CMD        8.0f    // unit(voltage)
-
-
-
-#define CONTROLLER          1     // 0 for transfer function 1 for control
+#include "PMW3901.h"
 
 Serial pc(USBTX,USBRX);
 InterruptIn mybutton(USER_BUTTON);
 Ticker main_function; //interrupt
-PwmOut pwm1A(D7);
-PwmOut pwm1B(D8);
-PwmOut pwm2A(D11);
-PwmOut pwm2B(A3);
+
 DigitalOut led1(LED1);
-
-//RX
-int readcount = 0;
-int RX_flag2 = 0;
-char getData[6] = {0,0,0,0,0,0};
-short data_received[2] = {0,0};
+//SPI speed_sensor(PC_12,PC_11,PC_10);
+SPI spi(PC_12,PC_11,PC_10);
+//DigitalOut CS(PA_4);
+DigitalOut cs(PA_4);
 
 float dt = 0.01; // sec
 float command = 0;
-float velocityA = 0; //rpm
-float velocityB = 0;
-float positionA = 0;
-float positionB = 0;
-short EncoderPositionA;
-short EncoderPositionB;
-float last_voltA = 0;
-float last_voltB = 0;
-float errorA = 0;
-float error_drA = 0;
-float errorB = 0;
-float error_drB = 0;
 bool button_state = false;
-float dutycycle = PWM_STOP;
-float VELOCITY_SPEED_A = 0.0;
-float VELOCITY_SPEED_B = 0.0;
-int pub_count = 0;
+
+/*void InitEncoder(void);*/
+
+void init_SPI();
+
+void start(void);
+void grabData(void);
+//void printData(void);
+void initializeSensor(void);
+void writeRegister(uint8_t addr, uint8_t data);
+uint8_t readRegister(uint8_t addr);
+void delayus(uint32_t us);
 
 void step_command();
-void position_control();
-float PD(float e, float last_e, float last_u, float P, float D);
-float PDF(float e, float last_e, float last_u, float P, float D, float F);
-void ReadVelocity();
-void ReadPosition(float *positionA, float *positionB);
-void motor_drive(float voltA, float voltB);
-void InitMotor(float period_in_ms);
-void InitEncoder(void);
-void control_speed();
 
-
-void RX_ITR();
-void init_UART();
-
+//void RX_ITR();
+//void init_UART();
 
 int main() {
     pc.baud(115200);
-    
-    InitEncoder();      //don't care
-    InitMotor(PWM_FREQUENCY); // Set pwm period to 1ms.
-    init_UART();
+    init_SPI();
+//    InitEncoder();      //don't care
+    //InitMotor(PWM_FREQUENCY); // Set pwm period to 1ms.
+    //init_UART();
     mybutton.fall(&step_command);
+    initializeSensor();
+    //main_function.attach_us(&position_control, dt*1000000);
+    main_function.attach_us(&start, dt*1000000);
 
-    main_function.attach_us(&position_control, dt*1000000);
-    
     while(1){}
 }
 
-void InitMotor(float period_in_us){
-    pwm1A.period_us(period_in_us);
-    pwm1B.period_us(period_in_us);
-    pwm1A.write(PWM_STOP);
-    pwm1B.write(PWM_STOP);
-    TIM1->CCER |= 0x0044;
-//    bool cc1ne_bit = (TIM1->CCER >> 2) & 0x0001;
-//    pc.printf("CC1NE bit : %d\r",cc1ne_bit);
+void start(){
+    cs = 0;
+    grabData();
+    //printData();
+    cs = 1;
 }
 
+void init_SPI(){
+    //CS.output = 1;
+    cs = 1;
+    //speed_sensor.format(8, 3);
+    //speed_sensor.frequency(1000000);
+    spi.format(8, 3);
+    spi.frequency(1000000);
+}
 
 void step_command(){
     led1 = !led1;
     button_state = !button_state;
-    
-//    // Do what you want motor to do.
-//    if(command == 0.0f){
-////        command = 8.0f; // volts  used to open loop control
-//        command = 90.0f; // deg     used to posiyion control
-//    }
-//    else{
-////        motor_drive(0.0f,0.0f);
-////        positionA = 0.0f;
-//        command = 0.0f;
-//    }
 }
 
-void position_control(){
-#if CONTROLLER == 0
-    if(button_state == true){
-        ReadVelocity();
-        command = VELOCITY_CMD;
-        //printf("%.3f, %.3f\r\n",command, velocityA);
-        motor_drive(command,0);
-    }else{
-        dutycycle = PWM_STOP;
-        pwm1A.write(dutycycle);
-        pwm1B.write(dutycycle);
-        TIM1->CCER |= 0x0044;
-        command = 0;
-        //printf("%.3f, %.3f\r\n",command, velocityA);  // velocityA or velocityB
-    }
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//=========================================================================
+//Functions definitions
+//=========================================================================
 
-    
-#endif
+uint8_t readRegister(uint8_t addr) {
+    wait_us(10);                                //tswr
+    //LL_GPIO_ResetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
+    cs = 0;
+    addr = addr & 0x7F;                         //Set MSB to 0 to indicate read operation
 
-#if CONTROLLER == 1
-    if(button_state == true){
-        pub_count++;
-        ReadVelocity();
-        control_speed();
-        if (pub_count >= 10){
-            printf("%.3f,%.3f\r\n",velocityA, velocityB);  // velocityA or velocityB
-            //printf("CMD %.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
-            pub_count = 0;
-        }
-    }else{
-        dutycycle = PWM_STOP;
-        pwm1A.write(dutycycle);
-        pwm1B.write(dutycycle);
-        TIM1->CCER |= 0x0044;
-        command = 0;
-        //printf("%.3f, %.3f\r\n",command, velocityA);  // velocityA or velocityB
-    }
-#endif
+//    LL_SPI_TransmitData16(SPI3, addr);
+//    while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) {   //Stuck if not finished
+//    }
+//    LL_SPI_ReceiveData16(SPI3);                 //Just to clear RXNE
+    spi.write(addr);
+    
+    wait_us(35);
+
+//    LL_SPI_TransmitData16(SPI3, 0U);
+//    while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) {   //Stuck if not finished
+//    }
+//    uint8_t data_read = LL_SPI_ReceiveData16(SPI3);
+    uint8_t data_read = spi.write(0U);
+    
+    wait_us(1);                             //tsclk-ncs
+    //LL_GPIO_SetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
+    cs = 1;
+    wait_us(20);                            //tsclk-ncs
+    return data_read;                           //Returns 8-bit data from register
 }
 
+//=========================================================================
+void writeRegister(uint8_t addr, uint8_t data) {
+    //LL_GPIO_ResetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
+    cs = 0;
+    addr = addr | 0x80;             //Set MSB to 1 to indicate write operation
 
-void ReadVelocity(){
-    /*
-    The velocity is calculated by follow :
-    velocity = EncoderPosition /Encoder CPR (Counts per round) /gear ratio *2pi /dt
-    unit : rad/sec
-    */
-    
-    EncoderPositionA = TIM2->CNT ;
-    EncoderPositionB = TIM3->CNT ;
-    TIM2->CNT = 0;
-    TIM3->CNT = 0;
-    // rad/s
-    velocityA = EncoderPositionA /HALL_RESOLUTION /GEAR_RATIO /dt *60;
-    velocityB = EncoderPositionB /HALL_RESOLUTION /GEAR_RATIO /dt *60;
-    // RPM
-//    *velocityA = EncoderPositionA /64.0 /90.0 /dt *60.0;
-//    *velocityB = EncoderPositionB /64.0 /90.0 /dt *60.0;
-}
+//    LL_SPI_TransmitData16(SPI3, addr);
+//    while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) {   //Stuck if not finished
+//    }
+//    LL_SPI_ReceiveData16(SPI3);             //Just to clear RXNE
+    spi.write(addr);
 
-
-void motor_drive(float voltA, float voltB){
-    // Input voltage is in range -12.0V ~ 12.0V
+//    LL_SPI_TransmitData16(SPI3, data);
+//    while (!LL_SPI_IsActiveFlag_RXNE(SPI3)) {   //Stuck if not finished
+//    }
+//    LL_SPI_ReceiveData16(SPI3);             //Just to clear RXNE
+    spi.write(data);
 
-    if(abs(voltA) <= minimum_volt){
-        if(voltA > 0){ voltA = minimum_volt; }
-        else{ voltA = -minimum_volt; }
-    }
-    
-    // Convet volt to pwm
-    
-    float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE;
-    float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE;
-    pwm1A.write(dutycycleA);
-    pwm1B.write(dutycycleB);
-    TIM1->CCER |= 0x0044;
+    wait_us(25);                            //tsclk-ncs
+
+    //LL_GPIO_SetOutputPin(SPI3_CS2_GPIO_Port, SPI3_CS2_Pin);
+    cs = 1;
+    wait_us(1);                             //tsclk-ncs
 }
 
-void control_speed(){
-    float voltA;
-    float voltB;
-    errorA = (VELOCITY_SPEED_A - velocityA);
-    voltA = last_voltA+0.4f*errorA-0.35f*error_drA;
-    error_drA = errorA;
-    last_voltA = voltA;
-    if(abs(voltA) <= minimum_volt){
-        if(voltA > 0){ voltA = minimum_volt; }
-        else{ voltA = -minimum_volt; }
-    }
-    if(abs(voltA) > INPUT_VOLTAGE){
-        if(voltA > 0){voltA = INPUT_VOLTAGE;}
-        else{voltA = -INPUT_VOLTAGE;}    
+//=========================================================================
+void initializeSensor(void) {
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x55, 0x01);
+    writeRegister(0x50, 0x07);
+    writeRegister(0x7F, 0x0E);
+    writeRegister(0x43, 0x10);
+
+    if (readRegister(0x67) & 0x40)
+        writeRegister(0x48, 0x04);
+    else
+        writeRegister(0x48, 0x02);
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x51, 0x7B);
+    writeRegister(0x50, 0x00);
+    writeRegister(0x55, 0x00);
+    writeRegister(0x7F, 0x0E);
+
+    if (readRegister(0x73) == 0x00) {
+        writeRegister(0x7F, 0x00);
+        writeRegister(0x61, 0xAD);
+        writeRegister(0x51, 0x70);
+        writeRegister(0x7F, 0x0E);
+
+        if (readRegister(0x70) <= 28)
+            writeRegister(0x70, readRegister(0x70) + 14);
+        else
+            writeRegister(0x70, readRegister(0x70) + 11);
+
+        writeRegister(0x71, readRegister(0x71) * 45 / 100);
     }
-    
-    errorB = (VELOCITY_SPEED_B - velocityB);
-    voltB = last_voltB+0.4f*errorB-0.35f*error_drB;
-    error_drB = errorB;
-    last_voltB = voltB;
-    if(abs(voltB) <= minimum_volt){
-        if(voltB > 0){ voltB = minimum_volt; }
-        else{ voltB = -minimum_volt; }
-    }
-    if(abs(voltB) > INPUT_VOLTAGE){
-        if(voltB > 0){voltB = INPUT_VOLTAGE;}
-        else{voltB = -INPUT_VOLTAGE;}    
-    }
-    
-    float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE;
-    float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE;
-    pwm1A.write(dutycycleA);
-    pwm1B.write(dutycycleB);
-    TIM1->CCER |= 0x0044;
-    //printf("%.3f, %.3f, %.3f\r\n",error1, last_error, voltA);
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x61, 0xAD);
+    writeRegister(0x7F, 0x03);
+    writeRegister(0x40, 0x00);
+    writeRegister(0x7F, 0x05);
+    writeRegister(0x41, 0xB3);
+    writeRegister(0x43, 0xF1);
+    writeRegister(0x45, 0x14);
+    writeRegister(0x5B, 0x32);
+    writeRegister(0x5F, 0x34);
+    writeRegister(0x7B, 0x08);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x44, 0x1B);
+    writeRegister(0x40, 0xBF);
+    writeRegister(0x4E, 0x3F);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x44, 0x1B);
+    writeRegister(0x40, 0xBF);
+    writeRegister(0x4E, 0x3F);
+    writeRegister(0x7F, 0x08);
+    writeRegister(0x65, 0x20);
+    writeRegister(0x6A, 0x18);
+    writeRegister(0x7F, 0x09);
+    writeRegister(0x4F, 0xAF);
+    writeRegister(0x5F, 0x40);
+    writeRegister(0x48, 0x80);
+    writeRegister(0x49, 0x80);
+    writeRegister(0x57, 0x77);
+    writeRegister(0x60, 0x78);
+    writeRegister(0x61, 0x78);
+    writeRegister(0x62, 0x08);
+    writeRegister(0x63, 0x50);
+    writeRegister(0x7F, 0x0A);
+    writeRegister(0x45, 0x60);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x4D, 0x11);
+    writeRegister(0x55, 0x80);
+    writeRegister(0x74, 0x21);
+    writeRegister(0x75, 0x1F);
+    writeRegister(0x4A, 0x78);
+    writeRegister(0x4B, 0x78);
+    writeRegister(0x44, 0x08);
+    writeRegister(0x45, 0x50);
+    writeRegister(0x64, 0xFF);
+    writeRegister(0x65, 0x1F);
+    writeRegister(0x7F, 0x14);
+    writeRegister(0x65, 0x67);
+    writeRegister(0x66, 0x08);
+    writeRegister(0x63, 0x70);
+    writeRegister(0x7F, 0x15);
+    writeRegister(0x48, 0x48);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x41, 0x0D);
+    writeRegister(0x43, 0x14);
+    writeRegister(0x4B, 0x0E);
+    writeRegister(0x45, 0x0F);
+    writeRegister(0x44, 0x42);
+    writeRegister(0x4C, 0x80);
+    writeRegister(0x7F, 0x10);
+    writeRegister(0x5B, 0x02);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x41);
+    writeRegister(0x70, 0x00);
+
+    wait_ms(10);
+
+    writeRegister(0x32, 0x44);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x40);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x62, 0xF0);
+    writeRegister(0x63, 0x00);
+    writeRegister(0x7F, 0x0D);
+    writeRegister(0x48, 0xC0);
+    writeRegister(0x6F, 0xD5);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x5B, 0xA0);
+    writeRegister(0x4E, 0xA8);
+    writeRegister(0x5A, 0x50);
+    writeRegister(0x40, 0x80);
+
+    wait_ms(250);
+
+    writeRegister(0x7F, 0x14);
+    writeRegister(0x6F, 0x1C);
+    writeRegister(0x7F, 0x00);
+
 }
 
-
-
-
-void InitEncoder(void) {
-    // Hardware Quadrature Encoder AB for Nucleo F446RE
-    // Output on debug port to host PC @ 9600 baud
-
-    /* Connections
-    PA_0 = Encoder1 A
-    PA_1 = Encoder1 B
-    PB_5 = Encoder2 A
-    PB_4 = Encoder2 B
-    */
+//=========================================================================
+void grabData(void) {
+    static int totalX = 0;
+    static int totalY = 0;
+    uint8_t check = 0;
+    if(button_state == true){
+        check = readRegister(0x02) & 0x80;
+        if (check) {
+            deltaX_low = readRegister(0x03);        //Grabs data from the proper registers.
+            deltaX_high = (readRegister(0x04) << 8) & 0xFF00; //Grabs data and shifts it to make space to be combined with lower bits.
+            deltaY_low = readRegister(0x05);
+            deltaY_high = (readRegister(0x06) << 8) & 0xFF00;
     
-    // configure GPIO PA0, PA1, PB5 & PB4 as inputs for Encoder
-    RCC->AHB1ENR |= 0x00000003;  // Enable clock for GPIOA & GPIOB
- 
-    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           // PA0 & PA1 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
-    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_0 | GPIO_PUPDR_PUPDR1_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
-    GPIOA->AFR[0]  |= 0x00000011 ;                                          // AF1 for PA0 & PA1                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-   
- 
-    GPIOB->MODER   |= GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 ;           // PB5 & PB4 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
-    GPIOB->PUPDR   |= GPIO_PUPDR_PUPDR4_0 | GPIO_PUPDR_PUPDR5_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
-    GPIOB->AFR[0]  |= 0x00220000 ;                                          // AF2 for PB5 & PB4                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-    GPIOB->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-   
-    // configure TIM2 & TIM3 as Encoder input
-    RCC->APB1ENR |= 0x00000003;  // Enable clock for TIM2 & TIM3
+            deltaY = deltaX_high | deltaX_low;      //Combines the low and high bits.
+            deltaX = deltaY_high | deltaY_low;
+    //        pc.printf("deltaX: %d\t\t\tdeltaY: %d\n\r", deltaX, deltaY);    //Prints each individual count of deltaX and deltaY.
+            //pc.printf("X-axis Counts: %d\t\tY-axis Counts: %d\n\r", totalX, totalY);  //Prints the total movement made during runtime.
+            totalX += deltaX;
+            totalY += deltaY;
 
-    TIM2->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1  
-    TIM2->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
-    TIM2->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
-    TIM2->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
-    TIM2->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
-    TIM2->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
-    TIM2->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
-  
-    TIM2->CNT = 0x0000;  //reset the counter before we use it
- 
-    TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1    
-    TIM3->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
-    TIM3->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
-    TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
-    TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
-    TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
-    TIM3->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
-  
-    TIM3->CNT = 0x0000;  //reset the counter before we use it
-}
+        }
 
-void init_UART()
-{
-    pc.baud(9600);  // baud rate設為9600
-    pc.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
-}
-
+        printf("%d,%d\n\r", totalX, totalY);
 
-void RX_ITR()
-{
-    while(pc.readable()) {
-        char uart_read;
-        uart_read = pc.getc();
-        if(uart_read == 115) {
-            RX_flag2 = 1;
-            readcount = 0;
-            getData[5] = 0;
-        } 
-        if(RX_flag2 == 1) {
-            getData[readcount] = uart_read;
-            readcount++;
-            if(readcount >= 6 & getData[5] == 101) {
-                readcount = 0;
-                RX_flag2 = 0;               
-                ///code for decoding///
-                data_received[0] = (getData[2] << 8) | getData[1];
-                data_received[1] = (getData[4] << 8) | getData[3];    
-                VELOCITY_SPEED_A = data_received[0]/100;
-                VELOCITY_SPEED_B = data_received[1]/100;
-                ///////////////////////
-            }
-        } 
     }
 }
\ No newline at end of file