Diff: main.cpp

Revision:

13:8e92260c63bd

Parent:

11:005a50dd7db5

--- a/main.cpp	Sun May 21 22:00:52 2017 +0000
+++ b/main.cpp	Mon May 22 14:17:58 2017 +0000
@@ -17,6 +17,9 @@
 // uncomment to send debug information
 #define DEBUG
 
+//Speed difference threshold to prevent CAN spam from throttle
+#define SPEED_THRESH 0.05
+
 //include 3rd party libraries
 #include "mbed.h" //needs to be revision 136 else SD filesystem will not work
 #include "rtos.h"
@@ -24,12 +27,16 @@
 // include Solarboat libraries
 #include "pinout.h"
 #include "PowerControl.h"
+#include "CAN_IDs.h"
 
 // initialize serial connection for debug
 #ifdef DEBUG
 RawSerial pc(SERIAL_TX, SERIAL_RX);
 #endif
 
+//initialize throttle 
+AnalogIn analog_throttle(STEER_THROTTLE);
+
 // initialize canbus
 CAN can(CAN_RD, CAN_TD);
 
@@ -44,6 +51,9 @@
 DigitalOut buckScreen(BUCK4);
 DigitalOut buck24V(BUCK5);
 
+//Motor functions
+void sendMotorSpeed(float throttle);
+void readThrottle();
 
 // Thread 1 - Power
 void power(){
@@ -67,10 +77,6 @@
     buckXSens = 0;
     buckScreen = 0;
     buck24V = 1;
-    
-    
-    
-    
 }
 
 // Thread 0 - DO NOT CHANGE THIS!
@@ -88,6 +94,7 @@
     //Thread thread3;
     //Thread thread4;
     Thread threadx;
+    Thread threadthrottle;
     
     // change thread priority
     //thread2.set_priority(osPriorityBelowNormal);
@@ -98,6 +105,7 @@
     //thread3.start(&motorTest);
     //thread4.start(&canReceive);
     threadx.start(&test);
+    threadthrottle.start(&readThrottle);
     
     //stop this thread while keeping the other threads running
     CANMessage msg;
@@ -107,4 +115,27 @@
         } 
     }
     Thread::wait(osWaitForever);
-}
\ No newline at end of file
+}
+
+//send motor command
+void sendMotorSpeed(float throttle) {
+    if ((abs(throttle - speed)) > SPEED_THRESH) {
+        union {
+            char msg[4];
+            float value;
+        } packet;
+        packet.value = throttle;
+        can.write(CANMessage(MOTOR_COMMAND, packet.msg));
+        printf("Sent motor speed: %f\r\n", packet.value);
+        speed = throttle;
+    }
+}
+
+//throttle thread
+void readThrottle() {
+    while(1) {
+        throttleread = 2*(0.5 - analog_throttle.read());
+        sendMotorSpeed(rev*throttleread);
+        Thread::wait(100);   
+    }
+} 
\ No newline at end of file