Diff: main.cpp

Revision:

0:eb99909151eb

Child:

1:b24bac622a37

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 17 05:34:19 2021 +0000
@@ -0,0 +1,258 @@
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "UserButton.h"
+#include "sMotor.h" 
+#include "lis3mdl_class.h"
+#include "QuadDisplay2.h"
+
+/****************************************************************
+*                           Definitions 
+****************************************************************/
+
+// Blinking rate in milliseconds
+#define step_speed 1500
+#define POINTS 1
+#define M_PI 3.14159265358979323846
+#define RADIAN M_PI/180.0
+
+/****************************************************************
+*                          Prototypes   
+*****************************************************************/
+
+void Ini();
+void HowManyClicks();
+void proximityR_isr();
+void proximityF_isr();
+//-----------------------
+void polling_sensors_isr();
+void sensors_task();
+//-----------------------
+void clk_speed_isr();
+/*****************************************************************
+*                           Interface
+******************************************************************/
+
+DigitalOut led1(LED1);
+DigitalIn CW_switch(D10);
+DigitalIn CCW_switch(D11);
+//---
+RawSerial pc(USBTX, USBRX);
+DevI2C devI2c(D14,D15);
+LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW);
+DigitalOut led2(A0);
+SPI spi(D4, D5, D3);
+QuadDisplayMy display(&spi,D6);
+//---
+sMotor motor(A5, A4, A3, A2); // creates new stepper motor: IN1, IN2, IN3, IN4
+InterruptIn BlueBatton(BUTTON)
+
+/*****************************************************************
+ *                            Time
+******************************************************************/
+//Ticker polling_sensors;
+//Ticker clk_speed;
+/****************************************************************
+*                           Threads  
+****************************************************************/
+//Thread sensor_daemon;
+
+/*****************************************************************
+*                             Global variables
+******************************************************************/
+
+
+//----------------
+uint8_t button_event;
+uint8_t mode=0;
+uint8_t RotateDir;
+uint16_t RotateSpeed;
+//----------------
+uint8_t SensorsEn=1;
+
+int16_t M[3];
+uint8_t point;
+uint32_t rotate_cnt;
+//---------
+//------------------
+
+/******************************************************************
+ *                              Main 
+******************************************************************/
+int main()
+{
+    // Initialise the digital pin LED1 as an output
+    
+    Ini();
+    
+    while (true) {
+        //led = !led;
+        HowManyClicks();
+        if (mode){
+            
+        }
+        if (point){
+            motor.step(POINTS, RotateDir, RotateSpeed);
+        }else
+           wait_us(step_speed);
+             
+        //thread_sleep_for(BLINKING_RATE_MS);
+    }
+}
+
+/**********************************************************
+*                              Functions  
+***********************************************************/
+void Ini()
+{
+ pc.baud(115200);
+ ButtonIni(&BlueBatton);
+ button_event_ind=button_event=0;
+ 
+ 
+//--------------
+
+sensor_daemon.start(sensors_task);
+SensorsEn=1;
+polling_sensors.attach(&polling_sensors_isr, 0.4);
+clk_speed.attach(&clk_speed_isr, 0.01);
+mode=0;
+point=0;
+RotateSpeed=step_speed;
+rotate_cnt=0;
+}
+
+//------------------
+void HowManyClicks()
+{
+    WhatButtonMode(&button_event)
+    if(button_event) {
+        switch (button_event) {
+            case 1:
+                mode=0;
+                point=0;
+                break;
+            case 2:
+                mode=1;
+                point=0;
+                //RotateDir=1;
+                //RotateSpeed=step_speed;
+                break;                    
+            case 3:
+                mode=2;
+                point=0;
+                //RotateDir=0;
+                //RotateSpeed=step_speed;
+                break;
+            case 4:
+                mode=3;
+                point=0;
+                //RotateDir=1;
+                //RotateSpeed=step_speed;
+                break;
+            case 5:
+                mode=4;
+                point=0;
+                //RotateSpeed=step_speed*2;
+                //RotateDir=0;
+                break;
+            case 6:
+                mode=5; 
+                point=1;
+                RotateSpeed=step_speed*2;
+                RotateDir=1; 
+                break;
+             case 7:
+                
+                break;               
+                
+            default:
+                break;                                                                                                   
+        }     
+        button_event=0;            
+    }  
+}
+
+//-----------------
+void sensors_task()
+{
+ int status;
+ char text[5];
+ int32_t dif; 
+ 
+    while (true) {
+        ThisThread::flags_wait_any(0x1,true);
+        SensorsEn=0;                 
+        status = board->sensor_left->get_distance(&distance_l); 
+        if (status != VL53L0X_ERROR_NONE)
+            distance_l=8888;  
+        status = board->sensor_right->get_distance(&distance_r);
+        if (status != VL53L0X_ERROR_NONE)
+             distance_r=8888;      
+        status = board->sensor_centre->get_distance(&distance_c); 
+        if (status != VL53L0X_ERROR_NONE)
+             distance_c=8888; 
+        switch(mode){
+            case 0:
+                sprintf(text,"c%3d",distance_c);  
+                break;
+            case 1:
+                sprintf(text,"l%3d", distance_l);                        
+                break;  
+            case 2:      
+                sprintf(text,"r%3d", distance_r);
+                break;    
+            case 3:
+                dif=((int)distance_r-(int)distance_l);   
+                sprintf(text,"d%3d",dif);
+                if (abs(dif)>4) {
+                    point=1;
+                    if(dif>0)
+                        RotateDir=1;
+                    else
+                        RotateDir=0;
+                }else    
+                    point=0;                       
+                break;   
+            case 4:      
+                sprintf(text,"%d",G[0]);
+                break;
+            case 5:      
+                sprintf(text,"%d",dur_time);
+                break;                 
+            default: 
+                sprintf(text,"----");                    
+                break;                      
+        } 
+        board->display->display_string(text);                                 
+        SensorsEn=1;
+   }         
+}
+
+/**********************************************************
+*                    Interrupt Service Routines 
+***********************************************************/
+void proximityR_isr()
+{
+  prox=1;      
+} 
+//---------------------
+void proximityF_isr()
+{ 
+  prox=0;      
+}    
+
+//---------------------
+void polling_sensors_isr()
+{
+ if (SensorsEn){
+    sensor_daemon.flags_set(0x1);
+    led = !led; 
+ }    
+}
+//---------------------------------
+void clk_speed_isr()
+{
+    rotate_cnt++; 
+    if (rotate_cnt>10)
+        dur_time_en=1;
+}