Diff: main.cpp

Revision:

0:a2b980143234

diff -r 000000000000 -r a2b980143234 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 10 14:21:44 2022 +0000
@@ -0,0 +1,101 @@
+#include "mbed.h"
+#include "PID.h"
+
+#define PULSE_PERIOD 1
+#define NUM_STEPS 800
+
+const PinName CANWritePin = p29;
+const PinName CANReadPin = p30;
+
+const int CANFrequency = 500000;
+const unsigned SteeringMessageID = 0x101;
+const unsigned SteeringRequestID = 0x100;
+
+Serial pc(USBTX, USBRX);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+PID SteeringController;
+
+DigitalOut S_DIR(p24);
+DigitalOut S_PUL(p23);
+
+union { double formattedData; char rawData[8]; } CANConversion;
+
+float steeringLocation = 0.0f;
+float steeringRequest = 0.0f;
+
+int TargetSteps = 0;
+
+CAN CANBus(CANReadPin, CANWritePin, CANFrequency);
+
+void AimForTarget() {
+     if (TargetSteps < -5) {
+        S_DIR = 0;
+        S_PUL = 1;
+        wait_ms(PULSE_PERIOD);
+        S_PUL = 0;
+        wait_ms(PULSE_PERIOD);
+        TargetSteps += 1;
+        //pc.printf("STEPS: %d \n\r", numSteps);
+    } else if (TargetSteps > 5){
+        S_DIR = 1; //CW
+        S_PUL = 1;
+        wait_ms(PULSE_PERIOD);
+        S_PUL = 0;
+        wait_ms(PULSE_PERIOD);
+        TargetSteps -= 1;
+        //pc.printf("STEPS: %d \n\r", numSteps);
+    }
+}
+
+void UpdateTarget() {
+    
+    CANMessage message;
+    
+    if (CANBus.read(message)) {
+        
+        if (message.id == SteeringMessageID) {
+            
+            for (int i = 0; i < 8; i++) {
+                CANConversion.rawData[i] = message.data[i];    
+            }
+    
+            steeringLocation = CANConversion.formattedData;
+            
+            led1 = !led1;
+        
+        } else if (message.id == SteeringRequestID) {
+            
+            for (int i = 0; i < 8; i++) {
+                CANConversion.rawData[i] = message.data[i];    
+            }
+    
+            steeringRequest = CANConversion.formattedData;
+            
+            led2 = !led2;
+            
+        }
+    }
+    
+    float error = (steeringRequest-steeringLocation);
+    
+    float steps = (error/360)*NUM_STEPS;
+    
+    TargetSteps = floor(steps);
+    
+    
+}
+
+int main() {
+    
+    pc.printf("STARTED new one\n\r");
+    CANBus.attach(callback(UpdateTarget));
+    
+    led1 = !led1;
+    
+    while (1) {
+        AimForTarget();
+    }
+    
+}