add all connections

controller_teensy/src/main.cpp

@@ -19,11 +19,28 @@ Tennsy Pin, Pin Name, Connected to
 #define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
 #define RECEIVEPERIOD 50 //ms
 
-#define PIN_THROTTLE A0
+#define PIN_THROTTLE A7
-const uint16_t calib_throttle_min = 350;
+const uint16_t calib_throttle_min = 400; //better a bit too high than too low
-const uint16_t calib_throttle_max = 810;
+const uint16_t calib_throttle_max = 790;
+#define PIN_BRAKE A8
+const uint16_t calib_brake_min = 100;//better a bit too high than too low
+const uint16_t calib_brake_max = 600;
+
+int16_t throttle_pos=0;
+int16_t brake_pos=0;
+
+unsigned long last_adcread=0;
+#define ADCREADPERIOD 100
+
+#define PIN_START A9
+#define PIN_LED_START 2 //Enginge start led
+
+#define PIN_LATCH_ENABLE A6
+
+#define PIN_MODE_SWITCH 3
+#define PIN_MODE_LEDG 4
+#define PIN_MODE_LEDR 5
 
-#define PIN_LED_START TODOOOOO //Enginge start led
 
 
 unsigned long last_send = 0;
@@ -176,8 +193,17 @@ void setup()
   Serial2.begin(SERIAL_CONTROL_BAUD); //control
   Serial3.begin(SERIAL_CONTROL_BAUD); //control
 
-  pinMode(PIN_THROTTLE, INPUT_PULLUP);
+  pinMode(PIN_THROTTLE, INPUT);
+  pinMode(PIN_BRAKE, INPUT);
+  pinMode(PIN_START, INPUT_PULLUP);
 
+  pinMode(PIN_LED_START, OUTPUT);
+  pinMode(PIN_MODE_LEDG, OUTPUT);
+  pinMode(PIN_MODE_LEDR, OUTPUT);
+
+  pinMode(PIN_LATCH_ENABLE, OUTPUT);
+  digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
+  pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
 }
 
 
@@ -189,6 +215,20 @@ void loop() {
   bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
 
   
+  if (loopmillis - last_adcread > ADCREADPERIOD) {
+    //read teensy adc and filter
+    last_adcread=loopmillis;
+    uint16_t throttle_raw = analogRead(PIN_THROTTLE);
+    throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
+    uint16_t brake_raw = analogRead(PIN_BRAKE);
+    brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
+
+    Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
+    Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
+    
+  }
+  
+  
   //Serial.print("fo="); Serial.println(count);
   //count++;
 
@@ -200,12 +240,9 @@ void loop() {
   }
 
   if (loopmillis - last_send > SENDPERIOD) {
+    //Calculate motor stuff and send to motor controllers
     last_send=loopmillis;
 
-    uint16_t throttle_raw = analogRead(PIN_THROTTLE);
-    int16_t throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
-       
-
     float freewheel_current=0.1;
     float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
     float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0;
@@ -229,7 +266,8 @@ void loop() {
 
     SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
 
-    Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
+    //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
+    
     
   }
 }