Ryan Savitski
2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: main.cpp
- Revision:
- 20:70d651156779
- Parent:
- 19:4b993a9a156e
- Child:
- 21:167dacfe0b14
- Child:
- 24:50805ef8c499
- Child:
- 43:c592bf6a6a2d
--- a/main.cpp Sun Apr 07 19:26:07 2013 +0000
+++ b/main.cpp Tue Apr 09 15:33:36 2013 +0000
@@ -1,77 +1,18 @@
-//#pragma Otime // Compiler Optimisations
-
-// Eurobot13 main.cpp
-
-
-
-/*
-PINOUT Sensors
-5: RF:SDI
-6 SDO
-7 SCK
-8 NCS
-9 NIRQ
-10-15 6 echo pins
-16 trig
-17 IRin
-18-20 unused
-21 stepper step
-22-27 unused
-28 Serial TX
-29-30 unused
-
-
-PINOUT Main
-5: Lower arm servo
-6: Upper arm servo
-
-14: Serial RX
-15: Distance sensor
-
-20: color sensor in
-21-24: Motors PWM IN 1-4
-25-26: Encoders
-27-28: Encoders
-29: Color sensor RED LED
-30: Color sensor BLUE LED
-
-*/
+#include "globals.h"
+#include "Kalman.h"
#include "mbed.h"
#include "rtos.h"
-#include "globals.h"
-Serial pc(USBTX, USBRX);
-
-const PinName P_SERVO_LOWER_ARM = p5;
-const PinName P_SERVO_UPPER_ARM = p6;
-
-const PinName P_SERIAL_RX = p14;
-const PinName P_DISTANCE_SENSOR = p15;
-
-const PinName P_COLOR_SENSOR_IN = p20;
-
-const PinName P_MOT_RIGHT_A = p21;
-const PinName P_MOT_RIGHT_B = p22;
-const PinName P_MOT_LEFT_A = p23;
-const PinName P_MOT_LEFT_B = p24;
-
-const PinName P_ENC_RIGHT_A = p28;
-const PinName P_ENC_RIGHT_B = p27;
-const PinName P_ENC_LEFT_A = p25;
-const PinName P_ENC_LEFT_B = p26;
-
-const PinName P_COLOR_SENSOR_RED = p29;
-const PinName P_COLOR_SENSOR_BLUE = p30;
-
-pos beaconpos[] = {{0,0}, {0,2}, {3,1}};
-
#include "Actuators/Arms/Arm.h"
#include "Actuators/MainMotors/MainMotor.h"
#include "Sensors/Encoders/Encoder.h"
#include "Sensors/Colour/Colour.h"
#include "Sensors/CakeSensor/CakeSensor.h"
#include "Processes/Printing/Printing.h"
+#include "coprocserial.h"
#include <algorithm>
+pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+
void motortest();
void encodertest();
void motorencodetest();
@@ -105,9 +46,9 @@
//ledphototransistortest();
//colourtest(); // Red SnR too low
//cakesensortest();
- feedbacktest();
+ //feedbacktest();
- /*
+ /*
DigitalOut l1(LED1);
Thread p(printingThread, NULL, osPriorityNormal, 2048);
l1=1;
@@ -116,11 +57,19 @@
Thread::wait(osWaitForever);
*/
- //Kalman test threads
- //Ticker predictticker;
- //predictthread(predictloopwrapper, this, osPriorityNormal, 512)
- //updatethread(updateloopwrapper, this, osPriorityNormal, 512)
- //predictticker( SIGTICKARGS(predictthread, 0x1) ),
+
+ InitSerial();
+ //while(1)
+ // printbuff();
+ wait(1);
+ Kalman::KalmanInit();
+
+ Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+
+ Kalman::start_predict_ticker(&predictthread);
+ //Thread::wait(osWaitForever);
+ feedbacktest();
+
}
#include <cstdlib>
@@ -132,7 +81,7 @@
registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
while (true){
for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
- buffer[i] =ID ;
+ buffer[i] = ID ;
}
updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
Thread::wait(200);
@@ -154,18 +103,21 @@
void feedbacktest(){
- Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
+ //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
- float Pgain = -0.005;
+ Kalman::State state;
+
+ float Pgain = -0.01;
float fwdspeed = -400/3.0f;
Timer timer;
timer.start();
while(true){
float expecdist = fwdspeed * timer.read();
- float errleft = Eleft.getPoint() - (expecdist*1.05);
- float errright = Eright.getPoint() - expecdist;
+ state = Kalman::getState();
+ float errleft = left_encoder.getTicks() - (expecdist);
+ float errright = right_encoder.getTicks() - expecdist;
mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
mright(max(min(errright*Pgain, 0.4f), -0.4f));
@@ -174,12 +126,12 @@
void cakesensortest(){
wait(1);
- pc.printf("cakesensortest");
+ printf("cakesensortest");
CakeSensor cs(P_COLOR_SENSOR_IN);
while(true){
wait(0.1);
- pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
+ printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
}
}
@@ -191,19 +143,19 @@
ColourEnum ce = c.getColour();
switch(ce){
case BLUE :
- pc.printf("BLUE\n\r");
+ printf("BLUE\n\r");
break;
case RED:
- pc.printf("RED\n\r");
+ printf("RED\n\r");
break;
case WHITE:
- pc.printf("WHITE\n\r");
+ printf("WHITE\n\r");
break;
case INCONCLUSIVE:
- pc.printf("INCONCLUSIVE\n\r");
+ printf("INCONCLUSIVE\n\r");
break;
default:
- pc.printf("BUG\n\r");
+ printf("BUG\n\r");
}
}
@@ -219,23 +171,23 @@
blue = 0; red = 0;
for(int i = 0; i != 5; i++){
wait(0.1);
- pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
+ printf("Phototransistor Analog is (none): %f \n\r", pt.read());
}
blue = 1; red = 0;
for(int i = 0; i != 5; i++){
wait(0.1);
- pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
+ printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
}
blue = 0; red = 1;
for(int i = 0; i != 5; i++){
wait(0.1);
- pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+ printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
}
blue = 1; red = 1;
for(int i = 0; i != 5; i++){
wait(0.1);
- pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+ printf("Phototransistor Analog is (both): %f \n\r", pt.read());
}
}
}
@@ -244,7 +196,7 @@
AnalogIn pt(P_COLOR_SENSOR_IN);
while(true){
wait(0.1);
- pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
+ printf("Phototransistor Analog is: %f \n\r", pt.read());
}
}
@@ -285,8 +237,8 @@
mleft(speed); mright(speed);
servoTimer.start();
while (true){
- pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
- if (Eleft.getPoint() < Eright.getPoint()){
+ printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
+ if (Eleft.getTicks() < Eright.getTicks()){
mleft(speed);
mright(speed - dspeed);
} else {
@@ -330,8 +282,8 @@
while (true){
//left 27 cm = 113 -> 0.239 cm/pulse
//right 27 cm = 72 -> 0.375 cm/pulse
- pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
- if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
+ printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
+ if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
mright(speed - dspeed);
} else {
mright(speed + dspeed);
@@ -349,13 +301,13 @@
const int enc = -38;
while(true){
mleft(speed); mright(0);
- while(Eleft.getPoint()>enc){
- pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+ while(Eleft.getTicks()>enc){
+ printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
}
Eleft.reset(); Eright.reset();
mleft(0); mright(speed);
- while(Eright.getPoint()>enc){
- pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+ while(Eright.getTicks()>enc){
+ printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
}
Eleft.reset(); Eright.reset();
}
@@ -367,7 +319,7 @@
Serial pc(USBTX, USBRX);
while(true){
wait(0.1);
- pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint());
+ printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
}
}