joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

main.cpp@5:01e1e68309ae, 2018-10-15 (annotated)

Committer:
je310
Date:
Mon Oct 15 18:30:20 2018 +0000
Revision:
5:01e1e68309ae
Parent:
4:778bc352c47f 
testing eigen;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mab5449 0:6b383744246e 1 #include "mbed.h"
je310 3:10fa3102c2d7 2 #include "odrive.h"
je310 3:10fa3102c2d7 3 #include "lwip-interface/EthernetInterface.h"
je310 3:10fa3102c2d7 4 #include "comms.h"
je310 3:10fa3102c2d7 5 #include <string>
je310 4:778bc352c47f 6 #include "calibration.h"
je310 4:778bc352c47f 7 #include "Axis.h"
je310 5:01e1e68309ae 8 #include "servoAxis.h"
je310 4:778bc352c47f 9 #include "kinematics.h"
je310 3:10fa3102c2d7 10 #include "BufferedSerial.h"
je310 5:01e1e68309ae 11 #include "syncTime.h"
je310 5:01e1e68309ae 12 #include "MPU6050.h"
je310 5:01e1e68309ae 13 #include "ESKF.h"
je310 5:01e1e68309ae 14 #define EXTPIN1 PB_5 //pwm spi1_mosii
je310 5:01e1e68309ae 15 #define EXTPIN2 PB_15 // PWM spi2 mosi
je310 5:01e1e68309ae 16 #define EXTPIN3 PB_13 //PWM spi2sclk
je310 5:01e1e68309ae 17 #define EXTPIN4 PB_12 //
je310 5:01e1e68309ae 18 #define EXTPIN5 PA_15 //pwm
je310 5:01e1e68309ae 19 #define EXTPIN6 PC_7 //RX6 PWM
je310 3:10fa3102c2d7 20 DigitalOut led1(LED1);
je310 3:10fa3102c2d7 21 DigitalOut homeGND(PF_5);
je310 3:10fa3102c2d7 22 DigitalIn homeSwitchA(PA_3);
je310 3:10fa3102c2d7 23 DigitalIn homeSwitchB(PC_0);
je310 3:10fa3102c2d7 24 DigitalIn homeSwitchC(PC_3);
je310 5:01e1e68309ae 25 DigitalOut accelPower(PA_6);
je310 3:10fa3102c2d7 26 DigitalIn trigger(PF_3);
je310 5:01e1e68309ae 27 BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX,1024);
je310 4:778bc352c47f 28 calVals calibration;
je310 5:01e1e68309ae 29 MPU6050 mpu;
je310 5:01e1e68309ae 30 I2C i2c(PB_9, PB_8);
je310 5:01e1e68309ae 31
je310 4:778bc352c47f 32
mab5449 0:6b383744246e 33
je310 3:10fa3102c2d7 34 using std::string;
je310 3:10fa3102c2d7 35 const int BROADCAST_PORT_T = 58080;
je310 3:10fa3102c2d7 36 const int BROADCAST_PORT_R = 58081;
je310 3:10fa3102c2d7 37 EthernetInterface eth;
je310 3:10fa3102c2d7 38
je310 5:01e1e68309ae 39 SyncTime timeTracker(0,0);
je310 5:01e1e68309ae 40 float batteryV = -1;
je310 5:01e1e68309ae 41 UDPSocket socket;
je310 5:01e1e68309ae 42 int isCon = 0;
je310 3:10fa3102c2d7 43 void transmit()
je310 3:10fa3102c2d7 44 {
je310 5:01e1e68309ae 45 while(isCon != 0) {
je310 5:01e1e68309ae 46 Thread::yield();
je310 5:01e1e68309ae 47 }
je310 3:10fa3102c2d7 48 string out_buffer = "very important data";
je310 3:10fa3102c2d7 49 SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
je310 3:10fa3102c2d7 50 fromRobot msg;
je310 3:10fa3102c2d7 51 msg.motor1.busVoltage = 69;
je310 5:01e1e68309ae 52 buffered_pc.printf("starting send loop");
je310 3:10fa3102c2d7 53 while (true) {
je310 3:10fa3102c2d7 54 msg.motor1.busVoltage += 1;
je310 5:01e1e68309ae 55 //int ret = socket.sendto(transmit, &msg, sizeof(msg));
je310 3:10fa3102c2d7 56 //printf("sendto return: %d\n", ret);
je310 3:10fa3102c2d7 57
je310 3:10fa3102c2d7 58 Thread::wait(100);
je310 3:10fa3102c2d7 59 }
je310 3:10fa3102c2d7 60 }
je310 5:01e1e68309ae 61 fromRobot pingMsg;
je310 3:10fa3102c2d7 62 void receive()
je310 3:10fa3102c2d7 63 {
je310 5:01e1e68309ae 64 eth.connect();
je310 5:01e1e68309ae 65
je310 5:01e1e68309ae 66 socket.open(&eth);
je310 5:01e1e68309ae 67
je310 5:01e1e68309ae 68 socket.set_blocking(false);
je310 5:01e1e68309ae 69
je310 5:01e1e68309ae 70
je310 5:01e1e68309ae 71
je310 5:01e1e68309ae 72 buffered_pc.printf("Ethernet returned %d \n\r", isCon);
je310 5:01e1e68309ae 73 Thread::wait(100);
je310 5:01e1e68309ae 74 if(isCon !=0) eth.disconnect();
je310 5:01e1e68309ae 75 while(isCon != 0) {
je310 5:01e1e68309ae 76 Thread::yield();
je310 5:01e1e68309ae 77 }
je310 5:01e1e68309ae 78 socket.open(&eth);
je310 5:01e1e68309ae 79 buffered_pc.printf("Ethernet is connected at %s \n\r", eth.get_ip_address());
je310 5:01e1e68309ae 80 Thread::wait(100);
je310 3:10fa3102c2d7 81 SocketAddress receive;
je310 5:01e1e68309ae 82 //UDPSocket socket(&eth);
je310 3:10fa3102c2d7 83 int bind = socket.bind(BROADCAST_PORT_R);
je310 5:01e1e68309ae 84 SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
je310 3:10fa3102c2d7 85 //printf("bind return: %d", bind);
je310 3:10fa3102c2d7 86
je310 3:10fa3102c2d7 87 char buffer[256];
je310 5:01e1e68309ae 88
je310 5:01e1e68309ae 89
je310 5:01e1e68309ae 90 buffered_pc.printf("starting receive loop");
je310 5:01e1e68309ae 91 rosTime now;
je310 5:01e1e68309ae 92 Timer debugTimer;
je310 5:01e1e68309ae 93
je310 3:10fa3102c2d7 94 while (true) {
je310 3:10fa3102c2d7 95 //printf("\nWait for packet...\n");
je310 3:10fa3102c2d7 96 int n = socket.recvfrom(&receive, buffer, sizeof(buffer));
je310 5:01e1e68309ae 97 if(n != NSAPI_ERROR_WOULD_BLOCK){
je310 5:01e1e68309ae 98 //debugTimer.start();
je310 3:10fa3102c2d7 99 toRobot inmsg;
je310 3:10fa3102c2d7 100 memcpy(&inmsg, buffer, n);
je310 5:01e1e68309ae 101 if(inmsg.isPing ==1) {
je310 5:01e1e68309ae 102 now = timeTracker.getTime();
je310 5:01e1e68309ae 103 if(now.seconds < 10000){
je310 5:01e1e68309ae 104 buffered_pc.printf("Hard resetting time to %ds and %dns \n\r",inmsg.time.seconds,inmsg.time.nSeconds);
je310 5:01e1e68309ae 105 timeTracker.hardReset(inmsg.time.seconds,inmsg.time.nSeconds);
je310 5:01e1e68309ae 106 }
je310 5:01e1e68309ae 107
je310 5:01e1e68309ae 108 pingMsg.isPing = 1;
je310 5:01e1e68309ae 109 pingMsg.pingTime.seconds = inmsg.time.seconds;
je310 5:01e1e68309ae 110 pingMsg.pingTime.nSeconds = inmsg.time.nSeconds;
je310 5:01e1e68309ae 111 pingMsg.time.seconds = now.seconds;
je310 5:01e1e68309ae 112 pingMsg.time.nSeconds = now.nSeconds;
je310 5:01e1e68309ae 113 int ret = socket.sendto(transmit, &pingMsg, sizeof(pingMsg));
je310 5:01e1e68309ae 114 //debugTimer.stop();
je310 5:01e1e68309ae 115
je310 5:01e1e68309ae 116 //buffered_pc.printf("sectionTime %d \r\n", debugTimer.read_high_resolution_us());
je310 5:01e1e68309ae 117
je310 5:01e1e68309ae 118 //buffered_pc.printf("current time is %ds and %dns \r\n", now.seconds, now.nSeconds);
je310 5:01e1e68309ae 119 }
je310 5:01e1e68309ae 120 if(inmsg.timeOffset !=0.0f){
je310 5:01e1e68309ae 121 //buffered_pc.printf("Applying offset %fs \n\r",inmsg.timeOffset);
je310 5:01e1e68309ae 122 timeTracker.updateTime(inmsg.timeOffset);
je310 5:01e1e68309ae 123 }
je310 3:10fa3102c2d7 124 //buffer[n] = '\0';
je310 5:01e1e68309ae 125 //buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);
je310 5:01e1e68309ae 126 }
je310 5:01e1e68309ae 127 //debugTimer.reset();
je310 5:01e1e68309ae 128 Thread::yield();
je310 3:10fa3102c2d7 129 //Thread::wait(1);
je310 3:10fa3102c2d7 130 }
je310 3:10fa3102c2d7 131 }
je310 3:10fa3102c2d7 132
je310 3:10fa3102c2d7 133 int getCountPos(BufferedSerial ser, int axis)
je310 3:10fa3102c2d7 134 {
je310 3:10fa3102c2d7 135
je310 3:10fa3102c2d7 136 }
mab5449 0:6b383744246e 137
je310 3:10fa3102c2d7 138 void runOdrive()
je310 3:10fa3102c2d7 139 {
je310 5:01e1e68309ae 140 //start servos on endEffector
je310 5:01e1e68309ae 141 ServoAxis pitch(EXTPIN1,30, -30, 1500, 1200.0/120.0);
je310 5:01e1e68309ae 142 ServoAxis yaw(EXTPIN2,30, -30, 1500, 1200.0/120.0);
je310 5:01e1e68309ae 143 // while(1) {
je310 5:01e1e68309ae 144 // pitch.setAngle(30);
je310 5:01e1e68309ae 145 // yaw.setAngle(30);
je310 5:01e1e68309ae 146 // Thread::wait(300);
je310 5:01e1e68309ae 147 // pitch.setAngle(-30);
je310 5:01e1e68309ae 148 // yaw.setAngle(-30);
je310 5:01e1e68309ae 149 // Thread::wait(300);
je310 5:01e1e68309ae 150 // }
je310 5:01e1e68309ae 151
je310 5:01e1e68309ae 152
je310 3:10fa3102c2d7 153 BufferedSerial ODSerial0(PC_12,PD_2);
je310 5:01e1e68309ae 154 ODSerial0.baud(921600);
je310 3:10fa3102c2d7 155 BufferedSerial ODSerial1(PD_5,PD_6);
je310 5:01e1e68309ae 156 ODSerial1.baud(921600);
je310 3:10fa3102c2d7 157 ODrive OD0(ODSerial0);
je310 3:10fa3102c2d7 158 ODrive OD1(ODSerial1);
je310 5:01e1e68309ae 159 Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
je310 5:01e1e68309ae 160 Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
je310 5:01e1e68309ae 161 Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
je310 5:01e1e68309ae 162 int error = 0;
je310 5:01e1e68309ae 163 error += A.test();
je310 5:01e1e68309ae 164 error +=B.test();
je310 5:01e1e68309ae 165 error +=C.test();
je310 5:01e1e68309ae 166 buffered_pc.printf("there were %d errors in the read/write\r\n",error);
je310 5:01e1e68309ae 167 Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
je310 5:01e1e68309ae 168 buffered_pc.printf("setting motors to idle\r\n");
je310 5:01e1e68309ae 169 kin.goIdle();
je310 5:01e1e68309ae 170 kin.goIdle();
je310 5:01e1e68309ae 171 error += kin.setSafeParams();
je310 5:01e1e68309ae 172 while(error) {}
je310 5:01e1e68309ae 173 // kin.goIdle();//for some reason we need to do it multiple times!
je310 5:01e1e68309ae 174 while(*A.homeSwitch_||*B.homeSwitch_ || *C.homeSwitch_) {
je310 5:01e1e68309ae 175 Thread::wait(5);
je310 5:01e1e68309ae 176 batteryV = OD1.readBattery();
je310 5:01e1e68309ae 177 } // wait till user
je310 5:01e1e68309ae 178 buffered_pc.printf("finding index\r\n");
je310 5:01e1e68309ae 179 kin.findIndex();
je310 5:01e1e68309ae 180 buffered_pc.printf("activating motors\r\n");
je310 5:01e1e68309ae 181 kin.activateMotors();
je310 5:01e1e68309ae 182 buffered_pc.printf("homing motors\r\n");
je310 5:01e1e68309ae 183 kin.homeMotors();
je310 5:01e1e68309ae 184 //int error = kin.goToPos(0,0,-10);
je310 5:01e1e68309ae 185
je310 5:01e1e68309ae 186 kin.goToAngles(pi/4,pi/4,pi/4);
je310 5:01e1e68309ae 187 Thread::wait(500);
je310 5:01e1e68309ae 188 error += kin.setFastParams();
je310 5:01e1e68309ae 189 while(error) {}
je310 5:01e1e68309ae 190 Thread::wait(1000);
je310 5:01e1e68309ae 191
je310 5:01e1e68309ae 192 Thread::wait(100);
je310 5:01e1e68309ae 193 float inc = pi /150;
je310 5:01e1e68309ae 194 float i = 0;
je310 5:01e1e68309ae 195 float k = -100;
je310 5:01e1e68309ae 196 int up = 0;
je310 5:01e1e68309ae 197 float min = 80;
je310 5:01e1e68309ae 198 float max =220;
je310 5:01e1e68309ae 199 float mid = min+max;
je310 5:01e1e68309ae 200 mid/= 2;
je310 5:01e1e68309ae 201 float span = mid - min;
je310 5:01e1e68309ae 202 float radius = 40;
je310 5:01e1e68309ae 203 int loopCounter = 0;
je310 3:10fa3102c2d7 204 while(true) {
je310 5:01e1e68309ae 205 loopCounter++;
je310 5:01e1e68309ae 206 int count = 0;
je310 5:01e1e68309ae 207 for(int j = 0 ; j < 100; j++) {
je310 5:01e1e68309ae 208
je310 5:01e1e68309ae 209 if(trigger == false) {
je310 5:01e1e68309ae 210 count++;
je310 5:01e1e68309ae 211 }
je310 5:01e1e68309ae 212 }
je310 5:01e1e68309ae 213 if(count>90) {
je310 5:01e1e68309ae 214 i += inc;
je310 5:01e1e68309ae 215 if(up == 1)k+=0.2;
je310 5:01e1e68309ae 216 else k-=0.2;
je310 5:01e1e68309ae 217 if(k >= -min) up = 0;
je310 5:01e1e68309ae 218 if(k <= -max) up = 1;
je310 5:01e1e68309ae 219 radius =0.6* (span - abs(abs(k) - mid));
je310 5:01e1e68309ae 220 //buffered_pc.printf("x,y,z = %f %f %f",radius*sin(i),radius*cos(i),k);
je310 5:01e1e68309ae 221 int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
je310 5:01e1e68309ae 222 pitch.setAngle(25.0*sin(i));
je310 5:01e1e68309ae 223 yaw.setAngle(25.0*cos(i));
je310 5:01e1e68309ae 224 //int error = kin.goToPos(0,0,k);
je310 5:01e1e68309ae 225 if(i > 2*pi) i = 0;
je310 5:01e1e68309ae 226
je310 5:01e1e68309ae 227 // buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
je310 5:01e1e68309ae 228 // buffered_pc.printf("k= %f\r\n",k);
je310 5:01e1e68309ae 229 }
je310 5:01e1e68309ae 230 Thread::wait(1);
je310 5:01e1e68309ae 231 if(loopCounter % 2000 == 0) {
je310 5:01e1e68309ae 232 batteryV = OD1.readBattery();
je310 5:01e1e68309ae 233 }
je310 3:10fa3102c2d7 234 }
je310 3:10fa3102c2d7 235
je310 3:10fa3102c2d7 236 }
je310 3:10fa3102c2d7 237
je310 5:01e1e68309ae 238 void accelThread()
je310 5:01e1e68309ae 239 {
je310 5:01e1e68309ae 240 //mpu.initMPU6050();
je310 5:01e1e68309ae 241 accelPower = 1;
je310 5:01e1e68309ae 242 uint8_t whoami = mpu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
je310 5:01e1e68309ae 243 buffered_pc.printf("I AM 0x%x\n\r", whoami);
je310 5:01e1e68309ae 244 buffered_pc.printf("I SHOULD BE 0x68\n\r");
je310 5:01e1e68309ae 245 mpu.resetMPU6050(); // Reset registers to default in preparation for device calibration
je310 5:01e1e68309ae 246 mpu.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
je310 5:01e1e68309ae 247 mpu.initMPU6050();
je310 5:01e1e68309ae 248 buffered_pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
je310 5:01e1e68309ae 249 Timer t;
je310 5:01e1e68309ae 250 t.start();
je310 5:01e1e68309ae 251 float sum = 0;
je310 5:01e1e68309ae 252 uint32_t sumCount = 0;
je310 5:01e1e68309ae 253 while(1) {
je310 5:01e1e68309ae 254 Thread::wait(1);
je310 5:01e1e68309ae 255
je310 5:01e1e68309ae 256 if(mpu.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
je310 5:01e1e68309ae 257 mpu.readAccelData(accelCount); // Read the x/y/z adc values
je310 5:01e1e68309ae 258 mpu.getAres();
je310 5:01e1e68309ae 259
je310 5:01e1e68309ae 260 // Now we'll calculate the accleration value into actual g's
je310 5:01e1e68309ae 261 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
je310 5:01e1e68309ae 262 ay = (float)accelCount[1]*aRes - accelBias[1];
je310 5:01e1e68309ae 263 az = (float)accelCount[2]*aRes - accelBias[2];
je310 5:01e1e68309ae 264
je310 5:01e1e68309ae 265 mpu.readGyroData(gyroCount); // Read the x/y/z adc values
je310 5:01e1e68309ae 266 mpu.getGres();
je310 5:01e1e68309ae 267
je310 5:01e1e68309ae 268 // Calculate the gyro value into actual degrees per second
je310 5:01e1e68309ae 269 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
je310 5:01e1e68309ae 270 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
je310 5:01e1e68309ae 271 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
je310 5:01e1e68309ae 272
je310 5:01e1e68309ae 273 tempCount = mpu.readTempData(); // Read the x/y/z adc values
je310 5:01e1e68309ae 274 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
je310 5:01e1e68309ae 275
je310 5:01e1e68309ae 276 // buffered_pc.printf("ax = %f", 1000*ax);
je310 5:01e1e68309ae 277 // buffered_pc.printf(" ay = %f", 1000*ay);
je310 5:01e1e68309ae 278 // buffered_pc.printf(" az = %f mg\n\r", 1000*az);
je310 5:01e1e68309ae 279 Now = t.read_us();
je310 5:01e1e68309ae 280 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
je310 5:01e1e68309ae 281 lastUpdate = Now;
je310 5:01e1e68309ae 282
je310 5:01e1e68309ae 283 sum += deltat;
je310 5:01e1e68309ae 284 sumCount++;
je310 5:01e1e68309ae 285
je310 5:01e1e68309ae 286 if(lastUpdate - firstUpdate > 10000000.0f) {
je310 5:01e1e68309ae 287 beta = 0.04; // decrease filter gain after stabilized
je310 5:01e1e68309ae 288 zeta = 0.015; // increasey bias drift gain after stabilized
je310 5:01e1e68309ae 289 }
je310 5:01e1e68309ae 290
je310 5:01e1e68309ae 291 // Pass gyro rate as rad/s
je310 5:01e1e68309ae 292 mpu.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
je310 5:01e1e68309ae 293
je310 5:01e1e68309ae 294 // Serial print and/or display at 0.5 s rate independent of data rates
je310 5:01e1e68309ae 295 //delt_t = t.read_ms() - count;
je310 5:01e1e68309ae 296
je310 5:01e1e68309ae 297 mpu.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
je310 5:01e1e68309ae 298
je310 5:01e1e68309ae 299 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3] );
je310 5:01e1e68309ae 300 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
je310 5:01e1e68309ae 301 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
je310 5:01e1e68309ae 302 pitch *= 180.0f / PI;
je310 5:01e1e68309ae 303 yaw *= 180.0f / PI;
je310 5:01e1e68309ae 304 roll *= 180.0f / PI;
je310 5:01e1e68309ae 305 // roll += 90;
je310 5:01e1e68309ae 306 // if(roll > 180) roll = -(roll - 180);
je310 5:01e1e68309ae 307 // if(roll < -180)roll = -(roll + 180);
je310 5:01e1e68309ae 308 buffered_pc.printf("roll= %f pitch = %f yaw = %f \n\r", roll,pitch,yaw);
je310 5:01e1e68309ae 309
je310 5:01e1e68309ae 310
je310 5:01e1e68309ae 311
je310 5:01e1e68309ae 312 }
je310 5:01e1e68309ae 313
je310 5:01e1e68309ae 314 }
je310 5:01e1e68309ae 315 }
je310 5:01e1e68309ae 316
je310 5:01e1e68309ae 317 void batteryThread()
je310 5:01e1e68309ae 318 {
je310 5:01e1e68309ae 319 while(1) {
je310 5:01e1e68309ae 320 Thread::wait(5000);
je310 5:01e1e68309ae 321 buffered_pc.printf("batteryVoltage = %f\n\r", batteryV);
je310 5:01e1e68309ae 322 }
je310 5:01e1e68309ae 323
je310 5:01e1e68309ae 324
je310 5:01e1e68309ae 325 }
je310 5:01e1e68309ae 326
je310 5:01e1e68309ae 327
je310 5:01e1e68309ae 328
mab5449 0:6b383744246e 329
je310 3:10fa3102c2d7 330 int main()
je310 3:10fa3102c2d7 331 {
je310 5:01e1e68309ae 332
je310 5:01e1e68309ae 333 // testing eigen run time.
je310 5:01e1e68309ae 334
je310 5:01e1e68309ae 335 ESKF ourESKF;
je310 5:01e1e68309ae 336 Vector3f acc, gyro,pos;
je310 5:01e1e68309ae 337 Quaternionf quat(0.25,0.25,0.25,0.25);
je310 5:01e1e68309ae 338 acc = Vector3f::Random();
je310 5:01e1e68309ae 339 gyro = Vector3f::Random();
je310 5:01e1e68309ae 340 pos = Vector3f::Random();
je310 5:01e1e68309ae 341 Timer eigenTimer;
je310 5:01e1e68309ae 342 eigenTimer.reset();
je310 5:01e1e68309ae 343 eigenTimer.start();
je310 5:01e1e68309ae 344 for(int i = 0; i < 1000; i ++){
je310 5:01e1e68309ae 345 ourESKF.predictionUpdate(acc,gyro, 0.001);
je310 5:01e1e68309ae 346 }
je310 5:01e1e68309ae 347 for(int i = 0; i < 100; i ++){
je310 5:01e1e68309ae 348 ourESKF.observeErrorState(pos,quat);
je310 5:01e1e68309ae 349 }
je310 5:01e1e68309ae 350 eigenTimer.stop();
je310 5:01e1e68309ae 351 buffered_pc.printf("the eigen took %f seconds",eigenTimer.read());
je310 5:01e1e68309ae 352
je310 4:778bc352c47f 353
je310 4:778bc352c47f 354 calibration.e = 58.095;
je310 4:778bc352c47f 355 calibration.f = 60.722;
je310 4:778bc352c47f 356 calibration.re = 150;
je310 4:778bc352c47f 357 calibration.rf = 143.0;
je310 4:778bc352c47f 358 calibration.Aoffset = 0.43 - 0.111701;
je310 4:778bc352c47f 359 calibration.Boffset = 0.43 - 0.111701;
je310 4:778bc352c47f 360 calibration.Coffset = 0.43 - 0.111701;
je310 4:778bc352c47f 361 calibration.gearRatio = 89.0/24.0;
je310 3:10fa3102c2d7 362 buffered_pc.baud(115200);
je310 3:10fa3102c2d7 363 buffered_pc.printf("hello\r\n");
je310 5:01e1e68309ae 364 i2c.frequency(400000);
je310 3:10fa3102c2d7 365 Thread transmitter;
je310 3:10fa3102c2d7 366 Thread receiver;
je310 3:10fa3102c2d7 367 Thread odriveThread;
je310 5:01e1e68309ae 368 Thread accel;
je310 5:01e1e68309ae 369 Thread timeUpdate;
je310 5:01e1e68309ae 370 Thread printBattery;
mab5449 0:6b383744246e 371
je310 4:778bc352c47f 372
je310 3:10fa3102c2d7 373 //set switches up
je310 3:10fa3102c2d7 374 homeGND = false;
je310 3:10fa3102c2d7 375 homeSwitchA.mode(PullUp);
je310 3:10fa3102c2d7 376 homeSwitchB.mode(PullUp);
je310 3:10fa3102c2d7 377 homeSwitchC.mode(PullUp);
je310 3:10fa3102c2d7 378 trigger.mode(PullUp);
je310 5:01e1e68309ae 379
je310 5:01e1e68309ae 380
je310 3:10fa3102c2d7 381
je310 3:10fa3102c2d7 382 //buffered_pc.printf("Controller IP Address is %s\r\n", eth.get_ip_address());
je310 3:10fa3102c2d7 383 Thread::wait(5000);
je310 3:10fa3102c2d7 384
je310 5:01e1e68309ae 385 transmitter.start(transmit);
je310 5:01e1e68309ae 386 receiver.start(receive);
je310 5:01e1e68309ae 387 //timeUpdate.start(timeUpdateLoop);
je310 3:10fa3102c2d7 388 odriveThread.start(runOdrive);
je310 5:01e1e68309ae 389 accel.start(accelThread);
je310 5:01e1e68309ae 390 printBattery.start(batteryThread);
je310 3:10fa3102c2d7 391
je310 3:10fa3102c2d7 392 while (true) {
je310 4:778bc352c47f 393 //led1 = !led1;
je310 3:10fa3102c2d7 394 Thread::wait(500);
je310 3:10fa3102c2d7 395 }
mab5449 0:6b383744246e 396 }