Diff: main.cpp

Revision:

14:0ad1bc8be76b

Parent:

11:005a50dd7db5

Child:

15:56b25cffa523

--- a/main.cpp	Sun May 21 22:00:52 2017 +0000
+++ b/main.cpp	Mon May 22 15:16:34 2017 +0000
@@ -23,8 +23,11 @@
 
 // include Solarboat libraries
 #include "pinout.h"
+#include "CAN_IDs.h"
 #include "PowerControl.h"
 
+#define SPEED_THRESH 0.05
+
 // initialize serial connection for debug
 #ifdef DEBUG
 RawSerial pc(SERIAL_TX, SERIAL_RX);
@@ -33,6 +36,9 @@
 // initialize canbus
 CAN can(CAN_RD, CAN_TD);
 
+//init throttle
+AnalogIn analogThrottle(STEER_THROTTLE);
+
 // initialze onboard leds
 DigitalOut ledError(LED3);
 DigitalOut ledSD(LED1);
@@ -68,11 +74,34 @@
     buckScreen = 0;
     buck24V = 1;
     
-    
-    
-    
 }
 
+float speed = 0;
+
+//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() {
+    float throttleread;
+    while(1) {
+        throttleread = floor(2*(0.5 - analogThrottle.read())*1000)/1000;
+        sendMotorSpeed(throttleread);
+        Thread::wait(100);   
+    }
+}  
+
 // Thread 0 - DO NOT CHANGE THIS!
 int main() {  
 #ifdef DEBUG
@@ -88,6 +117,7 @@
     //Thread thread3;
     //Thread thread4;
     Thread threadx;
+    Thread threadthrottle;
     
     // change thread priority
     //thread2.set_priority(osPriorityBelowNormal);
@@ -98,6 +128,7 @@
     //thread3.start(&motorTest);
     //thread4.start(&canReceive);
     threadx.start(&test);
+    threadthrottle.start(&readThrottle);
     
     //stop this thread while keeping the other threads running
     CANMessage msg;