Andy Rodriguez / Mbed 2 deprecated JAGBot

USMA Jumping Agile Ground Robot

Dependencies:   mbed Servo PPM_USMA UM7_USMA CAN_FIFO_USMA Roboteq_CANOpen_USMA

main.cpp@25:83e71ad747ce, 2020-08-25 (annotated)

Committer:
DGonz
Date:
Tue Aug 25 21:23:52 2020 +0000
Revision:
25:83e71ad747ce
Parent:
23:81a709626a96 
Working Summer 2020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DGonz 1:2aecd43e9e7b 1 /*
DGonz 1:2aecd43e9e7b 2 JAGBot main program
DGonz 1:2aecd43e9e7b 3 USMA Jumping Agile Ground Robot
DGonz 1:2aecd43e9e7b 4 Fall 2019
agentarod23 0:4c71421f2822 5
DGonz 1:2aecd43e9e7b 6 Authors:
DGonz 1:2aecd43e9e7b 7 Dr. Daniel J. Gonzalez - daniel.gonzalez@westpoint.edu
DGonz 1:2aecd43e9e7b 8 CDT Andy Rodriguez - andres.rodriguez@westpont.edu
DGonz 1:2aecd43e9e7b 9 CDT Josh Loyd - joshua.loyd@westpoint.edu
DGonz 1:2aecd43e9e7b 10 */
DGonz 1:2aecd43e9e7b 11 #include "main.h"
agentarod23 0:4c71421f2822 12
DGonz 20:053dbb6a7ef5 13 using namespace FastMath;
DGonz 20:053dbb6a7ef5 14
agentarod23 11:799693c71751 15 void setupServo() {
agentarod23 5:e480cc23ad42 16 float myRange = .000675;
agentarod23 6:b1d81a112343 17 float myDegrees = 90;
DGonz 20:053dbb6a7ef5 18 myservo0.calibrate(myRange, myDegrees);
DGonz 20:053dbb6a7ef5 19 myservo1.calibrate(myRange, myDegrees);
agentarod23 3:be7e8f2ca326 20 myservo2.calibrate(myRange, myDegrees);
DGonz 23:81a709626a96 21 myservo3.calibrate(myRange, myDegrees);
DGonz 20:053dbb6a7ef5 22 }
DGonz 20:053dbb6a7ef5 23
DGonz 20:053dbb6a7ef5 24 void setup(){
DGonz 20:053dbb6a7ef5 25 pcSerial.baud(115200);
DGonz 23:81a709626a96 26 pcSerial.printf("----------- Start! -----------\n");
DGonz 20:053dbb6a7ef5 27 //Create instance of PPM class
DGonz 20:053dbb6a7ef5 28 //Pass in interrupt pin, minimum output value, maximum output value, minimum pulse time from transmitter, maximum pulse time from transmitter, number of channels, throttle channel (used for failsafe)
DGonz 20:053dbb6a7ef5 29 ppmInputs = new PPM(PPMinterruptPin, 0, 1, 1000, 1900, 8, 3);
DGonz 20:053dbb6a7ef5 30
DGonz 20:053dbb6a7ef5 31 setupServo();
DGonz 23:81a709626a96 32 setupIMU(); //IMU Reading is handled through a Serial Interrupt
DGonz 20:053dbb6a7ef5 33
DGonz 20:053dbb6a7ef5 34 //set up timer
DGonz 20:053dbb6a7ef5 35 timer.start();
DGonz 20:053dbb6a7ef5 36 tPrev = timer.read_us()/1000000.0;
DGonz 20:053dbb6a7ef5 37 t = timer.read_us()/1000000.0;
DGonz 20:053dbb6a7ef5 38 dt = t - tPrev;
DGonz 20:053dbb6a7ef5 39
DGonz 20:053dbb6a7ef5 40 //------------------------------ Servo Out
DGonz 20:053dbb6a7ef5 41 myservo0.write(0.5);
DGonz 20:053dbb6a7ef5 42 myservo1.write(0.5);
DGonz 20:053dbb6a7ef5 43 myservo2.write(0.5);
DGonz 20:053dbb6a7ef5 44 myservo3.write(0.5);
DGonz 20:053dbb6a7ef5 45
DGonz 20:053dbb6a7ef5 46 //------------------------------ ODrive Out
DGonz 20:053dbb6a7ef5 47 odrv0axis0.write(0.5);
DGonz 20:053dbb6a7ef5 48 odrv0axis1.write(0.5);
DGonz 20:053dbb6a7ef5 49 odrv1axis0.write(0.5);
DGonz 20:053dbb6a7ef5 50 odrv1axis1.write(0.5);
DGonz 20:053dbb6a7ef5 51
agentarod23 3:be7e8f2ca326 52 }
agentarod23 3:be7e8f2ca326 53
DGonz 1:2aecd43e9e7b 54 int main() {
DGonz 15:eae9fb2279b8 55 if (CANTEST){
DGonz 23:81a709626a96 56 // CANTest();
agentarod23 14:bfe1aa066935 57 }else if(SERVOTEST){
DGonz 23:81a709626a96 58 // calibMain();
DGonz 15:eae9fb2279b8 59 }else if(RCTEST){
DGonz 20:053dbb6a7ef5 60 // RCTest();
DGonz 15:eae9fb2279b8 61 }else if(IMUTEST){
agentarod23 21:582d7e15e647 62 // IMUTest();
joshlhsoj1 7:8605925653cb 63 }else{
DGonz 23:81a709626a96 64 wait(0.5);
DGonz 20:053dbb6a7ef5 65 setup();
DGonz 23:81a709626a96 66 pcSerial.printf("----------- Loop Start! -----------\n");
DGonz 23:81a709626a96 67 wait(0.5);
DGonz 23:81a709626a96 68 pcSerial.printf("roll, pitch, rollDot, pitchDot, T1, T2, T3, T4, acc, state\n");
DGonz 20:053dbb6a7ef5 69 while(isRunning){
DGonz 20:053dbb6a7ef5 70 loop();
DGonz 1:2aecd43e9e7b 71 }
agentarod23 0:4c71421f2822 72 }
agentarod23 0:4c71421f2822 73 }
DGonz 20:053dbb6a7ef5 74
DGonz 23:81a709626a96 75 void getRCInputs(){
DGonz 23:81a709626a96 76 //------------------------------ Get RC Input
DGonz 23:81a709626a96 77 //Get channel data (mapped to between 0 and 1 as I passed these in as my max and min in the PPM constructor)
DGonz 23:81a709626a96 78 //The throttle channel will return -1 if the signal from the transmitter is lost (the throttle pulse time goes below the minimum pulse time passed into the PPM constructor)
DGonz 23:81a709626a96 79 ppmInputs->GetChannelData(rcCommandInputsRaw);
DGonz 23:81a709626a96 80
DGonz 23:81a709626a96 81 // Channels: LUD, RRL, RUD, LRL, Pot1, Pot2, LeftSwitch, Right trigger
DGonz 23:81a709626a96 82 for(int i = 0; i < 8; i++){
DGonz 23:81a709626a96 83 rcCommandInputsRaw[i]*=100;
DGonz 23:81a709626a96 84 rcCommandInputs[i]=rcCommandInputsRaw[i];
DGonz 23:81a709626a96 85 if(i==1){ //If RRL
DGonz 23:81a709626a96 86 rcCommandInputs[i] = map(rcCommandInputs[i], RRL_MIN, RRL_MAX,-100,100);
DGonz 23:81a709626a96 87 }else if(i==0){ //if LUD
DGonz 23:81a709626a96 88 rcCommandInputs[i] = map(rcCommandInputs[i], LUD_MIN, LUD_MAX,-100,100);
DGonz 23:81a709626a96 89 }else if(i==3){ //if LRL
DGonz 23:81a709626a96 90 rcCommandInputs[i] = map(rcCommandInputs[i], LRL_MIN, LRL_MAX,-100,100);
DGonz 23:81a709626a96 91 }else if(i==4 || i==5){ //if Pot1 or Pot 2
DGonz 23:81a709626a96 92 rcCommandInputs[i] = map(rcCommandInputs[i], LRL_MIN, LRL_MAX,0.0,1.0); //Do scaling factor for potentiometers
DGonz 23:81a709626a96 93 }
DGonz 23:81a709626a96 94
DGonz 23:81a709626a96 95 //Implement deadband
DGonz 23:81a709626a96 96 if(i>=0 && i<=3){
DGonz 23:81a709626a96 97 if(rcCommandInputs[i]<DEADBAND && rcCommandInputs[i]>-DEADBAND){
DGonz 23:81a709626a96 98 rcCommandInputs[i] = 0;
DGonz 23:81a709626a96 99 }else if(rcCommandInputs[i]>=DEADBAND){
DGonz 23:81a709626a96 100 rcCommandInputs[i] = map(rcCommandInputs[i], DEADBAND, 100, 0, 100);
DGonz 23:81a709626a96 101 }else if(rcCommandInputs[i]<=-DEADBAND){
DGonz 23:81a709626a96 102 rcCommandInputs[i] = map(rcCommandInputs[i], -100, -DEADBAND, -100, 00);
DGonz 23:81a709626a96 103 }
DGonz 23:81a709626a96 104 }
DGonz 23:81a709626a96 105 }
DGonz 23:81a709626a96 106 }
DGonz 23:81a709626a96 107
DGonz 23:81a709626a96 108 void scaleSkidSteer(){
DGonz 23:81a709626a96 109 bool flag = 0;
DGonz 23:81a709626a96 110 double maxVal = 0;
DGonz 23:81a709626a96 111 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 112 if(fabs(odrvCmds[i])>100 && fabs(odrvCmds[i])>maxVal){
DGonz 23:81a709626a96 113 flag = 1;
DGonz 23:81a709626a96 114 maxVal = fabs(odrvCmds[i]);
DGonz 23:81a709626a96 115 }
DGonz 23:81a709626a96 116 }
DGonz 23:81a709626a96 117 if(flag){
DGonz 23:81a709626a96 118 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 119 odrvCmds[i] = odrvCmds[i]*100.0/maxVal;
DGonz 23:81a709626a96 120 }
DGonz 23:81a709626a96 121 }
DGonz 23:81a709626a96 122 }
DGonz 23:81a709626a96 123
DGonz 20:053dbb6a7ef5 124 void loop(){
DGonz 20:053dbb6a7ef5 125 t = timer.read_us()/1000000.0;
DGonz 20:053dbb6a7ef5 126 loopCounter++;
DGonz 20:053dbb6a7ef5 127
DGonz 23:81a709626a96 128 //Take Average of IMU Readings (Handled by interrupts)
DGonz 23:81a709626a96 129 pitchAvg+=pitch;
DGonz 23:81a709626a96 130 rollAvg+=roll;
DGonz 23:81a709626a96 131 yawAvg+=yaw;
DGonz 23:81a709626a96 132 rollDotAvg+=rollDot;
DGonz 23:81a709626a96 133 pitchDotAvg+=pitchDot;
DGonz 23:81a709626a96 134 yawDotAvg+=yawDot;
DGonz 23:81a709626a96 135 accXAvg+=accXAvg;
DGonz 23:81a709626a96 136 accYAvg+=accYAvg;
DGonz 23:81a709626a96 137 accZAvg+=accZAvg;
agentarod23 21:582d7e15e647 138
DGonz 20:053dbb6a7ef5 139 if((t-tPrev)>=PERIOD){
DGonz 20:053dbb6a7ef5 140 dt = t - tPrev;
DGonz 20:053dbb6a7ef5 141 tPrev = t;
DGonz 20:053dbb6a7ef5 142 badTime = (dt>1.20*PERIOD); //If more than 20% off
DGonz 23:81a709626a96 143 getRCInputs();
DGonz 20:053dbb6a7ef5 144
DGonz 23:81a709626a96 145 pitchAvg/=loopCounter;
DGonz 23:81a709626a96 146 rollAvg/=loopCounter;
DGonz 23:81a709626a96 147 yawAvg/=loopCounter;
DGonz 23:81a709626a96 148 rollDotAvg/=loopCounter;
DGonz 23:81a709626a96 149 pitchDotAvg/=loopCounter;
DGonz 23:81a709626a96 150 yawDotAvg/=loopCounter;
DGonz 23:81a709626a96 151 accXAvg/=loopCounter;
DGonz 23:81a709626a96 152 accYAvg/=loopCounter;
DGonz 23:81a709626a96 153 accZAvg/=loopCounter;
DGonz 20:053dbb6a7ef5 154
DGonz 20:053dbb6a7ef5 155 if(CONTROL_MODE == 0){ //Do nothing
DGonz 23:81a709626a96 156 //---- Servo Out
DGonz 20:053dbb6a7ef5 157 myservo0.write(0.5);
DGonz 20:053dbb6a7ef5 158 myservo1.write(0.5);
DGonz 20:053dbb6a7ef5 159 myservo2.write(0.5);
DGonz 20:053dbb6a7ef5 160 myservo3.write(0.5);
DGonz 23:81a709626a96 161 //---- ODrive Out
DGonz 20:053dbb6a7ef5 162 odrv0axis0.write(0.5);
DGonz 20:053dbb6a7ef5 163 odrv0axis1.write(0.5);
DGonz 20:053dbb6a7ef5 164 odrv1axis0.write(0.5);
DGonz 20:053dbb6a7ef5 165 odrv1axis1.write(0.5);
DGonz 20:053dbb6a7ef5 166 }else if(CONTROL_MODE==1){ //Pure Skid Steer
DGonz 23:81a709626a96 167 //---- Servo Out
DGonz 20:053dbb6a7ef5 168 myservo0.write(0.5);
DGonz 20:053dbb6a7ef5 169 myservo1.write(0.5);
DGonz 20:053dbb6a7ef5 170 myservo2.write(0.5);
DGonz 20:053dbb6a7ef5 171 myservo3.write(0.5);
DGonz 23:81a709626a96 172 //---- ODrive Out
DGonz 20:053dbb6a7ef5 173 odrvCmds[0] = rcCommandInputs[3] + rcCommandInputs[0]; // LRL+LUD
DGonz 20:053dbb6a7ef5 174 odrvCmds[1] = -rcCommandInputs[3] + rcCommandInputs[0];
DGonz 20:053dbb6a7ef5 175 odrvCmds[2] = rcCommandInputs[3] + rcCommandInputs[0];
DGonz 20:053dbb6a7ef5 176 odrvCmds[3] = -rcCommandInputs[3] + rcCommandInputs[0];
DGonz 20:053dbb6a7ef5 177
DGonz 23:81a709626a96 178 //Skid Steer Scaling
DGonz 23:81a709626a96 179 scaleSkidSteer();
DGonz 20:053dbb6a7ef5 180
DGonz 23:81a709626a96 181 //Final Scaling with Potentiometer
DGonz 20:053dbb6a7ef5 182 for(int i=0; i<4; i++){
DGonz 20:053dbb6a7ef5 183 odrvCmds[i]*=rcCommandInputs[4];
DGonz 20:053dbb6a7ef5 184 }
DGonz 20:053dbb6a7ef5 185
DGonz 20:053dbb6a7ef5 186 odrv0axis0.write(map(odrvCmds[0],-100,100,0,1));
DGonz 20:053dbb6a7ef5 187 odrv0axis1.write(map(-odrvCmds[1],-100,100,0,1));
DGonz 20:053dbb6a7ef5 188 odrv1axis0.write(map(-odrvCmds[2],-100,100,0,1));
DGonz 20:053dbb6a7ef5 189 odrv1axis1.write(map(odrvCmds[3],-100,100,0,1));
DGonz 23:81a709626a96 190 }else if(CONTROL_MODE==2){ //OmniSteer
DGonz 23:81a709626a96 191 // Channels: LUD, RRL, RUD, LRL, Pot1, Pot2, LeftSwitch, Right trigger
DGonz 23:81a709626a96 192 // Channels: LUD, RRL, RUD, LRL, Pot1, Pot2, LeftSwitch, Right trigger
DGonz 23:81a709626a96 193 if(rcCommandInputs[0]==0 && rcCommandInputs[3]==0 && rcCommandInputs[1]!=0 ){ //Turn on Dime
DGonz 23:81a709626a96 194 myservo0.write(0.75);
DGonz 23:81a709626a96 195 myservo1.write(0.25);
DGonz 23:81a709626a96 196 myservo2.write(0.75);
DGonz 23:81a709626a96 197 myservo3.write(0.25);
DGonz 23:81a709626a96 198 odrvCmds[0] = -rcCommandInputs[1];
DGonz 23:81a709626a96 199 odrvCmds[1] = rcCommandInputs[1];
DGonz 23:81a709626a96 200 odrvCmds[2] = rcCommandInputs[1];
DGonz 23:81a709626a96 201 odrvCmds[3] = -rcCommandInputs[1];
DGonz 23:81a709626a96 202 }else if(rcCommandInputs[1]==0 && rcCommandInputs[3]!=0 && rcCommandInputs[0]==0){ //Strafe Left-Right
DGonz 23:81a709626a96 203 myservo0.write(1);
DGonz 23:81a709626a96 204 myservo1.write(0);
DGonz 23:81a709626a96 205 myservo2.write(1);
DGonz 23:81a709626a96 206 myservo3.write(0);
DGonz 23:81a709626a96 207 odrvCmds[0] = -rcCommandInputs[3];
DGonz 23:81a709626a96 208 odrvCmds[1] = rcCommandInputs[3];
DGonz 23:81a709626a96 209 odrvCmds[2] = -rcCommandInputs[3];
DGonz 23:81a709626a96 210 odrvCmds[3] = rcCommandInputs[3];
DGonz 23:81a709626a96 211 }else{ // Steer while driving
DGonz 23:81a709626a96 212 myservo0.write(0.5 - map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 213 myservo1.write(0.5 - map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 214 myservo2.write(0.5 + map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 215 myservo3.write(0.5 + map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 216 odrvCmds[0] = -0.1*rcCommandInputs[1] + rcCommandInputs[0]; //LRL+LUD, 3 and 0
DGonz 23:81a709626a96 217 odrvCmds[1] = 0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 218 odrvCmds[2] = 0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 219 odrvCmds[3] = -0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 220 }
DGonz 23:81a709626a96 221
DGonz 23:81a709626a96 222 //Skid Steer Scaling
DGonz 23:81a709626a96 223 scaleSkidSteer();
DGonz 23:81a709626a96 224
DGonz 23:81a709626a96 225 //Final Scaling with Potentiometer
DGonz 23:81a709626a96 226 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 227 odrvCmds[i]*=rcCommandInputs[4];
DGonz 23:81a709626a96 228 map(odrvCmds[i],-25,25,-25,25);
DGonz 23:81a709626a96 229 }
DGonz 23:81a709626a96 230
DGonz 23:81a709626a96 231 odrv0axis0.write(map(odrvCmds[0],-100,100,0,1));
DGonz 23:81a709626a96 232 odrv0axis1.write(map(-odrvCmds[1],-100,100,0,1));
DGonz 23:81a709626a96 233 odrv1axis0.write(map(-odrvCmds[2],-100,100,0,1));
DGonz 23:81a709626a96 234 odrv1axis1.write(map(odrvCmds[3],-100,100,0,1));
DGonz 23:81a709626a96 235 }else if(CONTROL_MODE==3){ //Only Balance
DGonz 23:81a709626a96 236 double kP1 = 10;
DGonz 23:81a709626a96 237 double kD1 = 25;
DGonz 23:81a709626a96 238 double kP2 = 10;
DGonz 23:81a709626a96 239 double kD2 = 25;
DGonz 23:81a709626a96 240 Tx = (kP1*(0 - roll) + kD1*(0 - rollDot))*DEG2RAD;
DGonz 23:81a709626a96 241 Ty = (kP2*(0 - pitch) + kD2*(0 - pitchDot))*DEG2RAD;
DGonz 23:81a709626a96 242
DGonz 23:81a709626a96 243 myservo0.write(0.75);
DGonz 23:81a709626a96 244 myservo1.write(0.25);
DGonz 23:81a709626a96 245 myservo2.write(0.75);
DGonz 23:81a709626a96 246 myservo3.write(0.25);
DGonz 23:81a709626a96 247 odrvCmds[0] = KV*HSIN*(Ty + Tx)/2;
DGonz 23:81a709626a96 248 odrvCmds[1] = KV*HSIN*(Ty - Tx)/2;
DGonz 23:81a709626a96 249 odrvCmds[2] = KV*HSIN*(Ty + Tx)/2;
DGonz 23:81a709626a96 250 odrvCmds[3] = KV*HSIN*(Ty - Tx)/2;
DGonz 23:81a709626a96 251
DGonz 23:81a709626a96 252 //Final Scaling with Potentiometer
DGonz 23:81a709626a96 253 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 254 odrvCmds[i]*=rcCommandInputs[4];
DGonz 23:81a709626a96 255 map(odrvCmds[i],-25,25,-25,25);
DGonz 23:81a709626a96 256 }
DGonz 23:81a709626a96 257
DGonz 23:81a709626a96 258 odrv0axis0.write(map(odrvCmds[0],-25,25,0,1));
DGonz 23:81a709626a96 259 odrv0axis1.write(map(-odrvCmds[1],-25,25,0,1));
DGonz 23:81a709626a96 260 odrv1axis0.write(map(-odrvCmds[2],-25,25,0,1));
DGonz 23:81a709626a96 261 odrv1axis1.write(map(odrvCmds[3],-25,25,0,1));
DGonz 23:81a709626a96 262 }else if(CONTROL_MODE==4){ //State Machine
DGonz 23:81a709626a96 263 if(state==0){ //Omnisteer
DGonz 23:81a709626a96 264 // Channels: LUD, RRL, RUD, LRL, Pot1, Pot2, LeftSwitch, Right trigger
DGonz 23:81a709626a96 265 if(rcCommandInputs[0]==0 && rcCommandInputs[3]==0 && rcCommandInputs[1]!=0 ){ //Turn on Dime
DGonz 23:81a709626a96 266 myservo0.write(0.75);
DGonz 23:81a709626a96 267 myservo1.write(0.25);
DGonz 23:81a709626a96 268 myservo2.write(0.75);
DGonz 23:81a709626a96 269 myservo3.write(0.25);
DGonz 23:81a709626a96 270 odrvCmds[0] = -rcCommandInputs[1];
DGonz 23:81a709626a96 271 odrvCmds[1] = rcCommandInputs[1];
DGonz 23:81a709626a96 272 odrvCmds[2] = rcCommandInputs[1];
DGonz 23:81a709626a96 273 odrvCmds[3] = -rcCommandInputs[1];
DGonz 23:81a709626a96 274 }else if(rcCommandInputs[1]==0 && rcCommandInputs[3]!=0 && rcCommandInputs[0]==0){ //Strafe Left-Right
DGonz 23:81a709626a96 275 myservo0.write(1);
DGonz 23:81a709626a96 276 myservo1.write(0);
DGonz 23:81a709626a96 277 myservo2.write(1);
DGonz 23:81a709626a96 278 myservo3.write(0);
DGonz 23:81a709626a96 279 odrvCmds[0] = -rcCommandInputs[3];
DGonz 23:81a709626a96 280 odrvCmds[1] = rcCommandInputs[3];
DGonz 23:81a709626a96 281 odrvCmds[2] = -rcCommandInputs[3];
DGonz 23:81a709626a96 282 odrvCmds[3] = rcCommandInputs[3];
DGonz 23:81a709626a96 283 }else{ // Steer while driving
DGonz 23:81a709626a96 284 myservo0.write(0.5 - map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 285 myservo1.write(0.5 - map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 286 myservo2.write(0.5 + map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 287 myservo3.write(0.5 + map(rcCommandInputs[1],-100,100,-0.5,0.5));
DGonz 23:81a709626a96 288 odrvCmds[0] = -0.1*rcCommandInputs[1] + rcCommandInputs[0]; //LRL+LUD, 3 and 0
DGonz 23:81a709626a96 289 odrvCmds[1] = 0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 290 odrvCmds[2] = 0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 291 odrvCmds[3] = -0.1*rcCommandInputs[1] + rcCommandInputs[0];
DGonz 23:81a709626a96 292 }
DGonz 23:81a709626a96 293
DGonz 23:81a709626a96 294 //Skid Steer Scaling
DGonz 23:81a709626a96 295 scaleSkidSteer();
DGonz 23:81a709626a96 296
DGonz 23:81a709626a96 297 //Final Scaling with Potentiometer
DGonz 23:81a709626a96 298 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 299 odrvCmds[i]*=rcCommandInputs[4];
DGonz 23:81a709626a96 300 map(odrvCmds[i],-25,25,-25,25);
DGonz 23:81a709626a96 301 }
DGonz 23:81a709626a96 302
DGonz 23:81a709626a96 303 odrv0axis0.write(map(odrvCmds[0],-100,100,0,1));
DGonz 23:81a709626a96 304 odrv0axis1.write(map(-odrvCmds[1],-100,100,0,1));
DGonz 23:81a709626a96 305 odrv1axis0.write(map(-odrvCmds[2],-100,100,0,1));
DGonz 23:81a709626a96 306 odrv1axis1.write(map(odrvCmds[3],-100,100,0,1));
DGonz 23:81a709626a96 307
DGonz 23:81a709626a96 308 if(rcCommandInputs[5]*sqrt((accX*accX + accY*accY + accZ*accZ))<0.5){ //units are gs
DGonz 23:81a709626a96 309 state = 1; //If we are in freefall, transition to aerial balance state.
DGonz 23:81a709626a96 310 tAirStart = t;
DGonz 23:81a709626a96 311 }
DGonz 23:81a709626a96 312 }else if(state==1){ //Aerial Balance
DGonz 23:81a709626a96 313 double kP1 = 75;
DGonz 23:81a709626a96 314 double kD1 = 12;
DGonz 23:81a709626a96 315 double kP2 = 75;
DGonz 23:81a709626a96 316 double kD2 = 12;
DGonz 23:81a709626a96 317 Tx = (kP1*(-1.3 - roll) + kD1*(0 - rollDot))*DEG2RAD;
DGonz 23:81a709626a96 318 Ty = (kP2*(1.5 - pitch) + kD2*(0 - pitchDot))*DEG2RAD;
DGonz 23:81a709626a96 319
DGonz 23:81a709626a96 320 myservo0.write(0.75);
DGonz 23:81a709626a96 321 myservo1.write(0.25);
DGonz 23:81a709626a96 322 myservo2.write(0.75);
DGonz 23:81a709626a96 323 myservo3.write(0.25);
DGonz 23:81a709626a96 324 odrvCmds[0] = KV*HSIN*(Ty + Tx)/2;
DGonz 23:81a709626a96 325 odrvCmds[1] = KV*HSIN*(Ty - Tx)/2;
DGonz 23:81a709626a96 326 odrvCmds[2] = KV*HSIN*(Ty + Tx)/2;
DGonz 23:81a709626a96 327 odrvCmds[3] = KV*HSIN*(Ty - Tx)/2;
DGonz 23:81a709626a96 328
DGonz 23:81a709626a96 329 //Final Scaling with Potentiometer
DGonz 23:81a709626a96 330 for(int i=0; i<4; i++){
DGonz 23:81a709626a96 331 odrvCmds[i]*=rcCommandInputs[4];
DGonz 23:81a709626a96 332 odrvCmds[i] = map(odrvCmds[i],-25,25,-25,25);
DGonz 23:81a709626a96 333 }
DGonz 23:81a709626a96 334
DGonz 23:81a709626a96 335 odrv0axis0.write(map(odrvCmds[0],-25,25,0,1));
DGonz 23:81a709626a96 336 odrv0axis1.write(map(-odrvCmds[1],-25,25,0,1));
DGonz 23:81a709626a96 337 odrv1axis0.write(map(-odrvCmds[2],-25,25,0,1));
DGonz 23:81a709626a96 338 odrv1axis1.write(map(odrvCmds[3],-25,25,0,1));
DGonz 23:81a709626a96 339
DGonz 23:81a709626a96 340 // if(rcCommandInputs[5]*sqrt((accX*accX + accY*accY + accZ*accZ))>4){ //units are m/(s^2)
DGonz 23:81a709626a96 341 if((rcCommandInputs[5]*sqrt((accX*accX + accY*accY + accZ*accZ))>2)||(t-tAirStart>1.5)){ //units are m/(s^2)
DGonz 23:81a709626a96 342 state = 2; //If we land, transition back to omnidirectional control
DGonz 23:81a709626a96 343 }
DGonz 23:81a709626a96 344 }else if(state==2){ //Land
DGonz 23:81a709626a96 345 myservo0.write(0.75);
DGonz 23:81a709626a96 346 myservo1.write(0.25);
DGonz 23:81a709626a96 347 myservo2.write(0.75);
DGonz 23:81a709626a96 348 myservo3.write(0.25);
DGonz 23:81a709626a96 349 odrvCmds[0] = 0;
DGonz 23:81a709626a96 350 odrvCmds[1] = 0;
DGonz 23:81a709626a96 351 odrvCmds[2] = 0;
DGonz 23:81a709626a96 352 odrvCmds[3] = 0;
DGonz 23:81a709626a96 353 odrv0axis0.write(map(0,-100,100,0,1));
DGonz 23:81a709626a96 354 odrv0axis1.write(map(0,-100,100,0,1));
DGonz 23:81a709626a96 355 odrv1axis0.write(map(0,-100,100,0,1));
DGonz 23:81a709626a96 356 odrv1axis1.write(map(0,-100,100,0,1));
DGonz 23:81a709626a96 357 }
DGonz 20:053dbb6a7ef5 358 }
DGonz 20:053dbb6a7ef5 359
DGonz 20:053dbb6a7ef5 360 //Telemetry
DGonz 20:053dbb6a7ef5 361 teleCounter++;
DGonz 20:053dbb6a7ef5 362 if(teleCounter > SERIAL_RATIO and USESERIAL and !inInt){ //and pcSerial.writable()
DGonz 20:053dbb6a7ef5 363 //inWrite = 1;
DGonz 20:053dbb6a7ef5 364 if(badTime){
DGonz 20:053dbb6a7ef5 365 pcSerial.printf("b%f\n",dt-1.25*PERIOD); //(dt-PERIOD)
DGonz 20:053dbb6a7ef5 366 badTime = 0;
DGonz 20:053dbb6a7ef5 367 }
DGonz 23:81a709626a96 368 teleCounter = 0;
DGonz 23:81a709626a96 369 // pcSerial.printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n", rcCommandInputsRaw[0], rcCommandInputsRaw[1], rcCommandInputsRaw[2], rcCommandInputsRaw[3], rcCommandInputsRaw[4], rcCommandInputsRaw[5], rcCommandInputsRaw[6], rcCommandInputsRaw[7]);
DGonz 23:81a709626a96 370 // pcSerial.printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, \n", rcCommandInputs[0], rcCommandInputs[1], rcCommandInputs[3], rcCommandInputs[4], rcCommandInputs[5], odrvCmds[0], odrvCmds[1], odrvCmds[2], odrvCmds[3]);
DGonz 23:81a709626a96 371 // pcSerial.printf("%f, %f, %f, \n", roll, pitch, yaw);
DGonz 23:81a709626a96 372 // pcSerial.printf("%f, %f, %f, ", rollDot, pitchDot, yawDot);
DGonz 23:81a709626a96 373 // pcSerial.printf("%f, %f, %f, %f, %f, %f, %f, %f, %i\n",
DGonz 23:81a709626a96 374 // accX, accY, accZ, accXAvg, accYAvg, accZAvg,
DGonz 23:81a709626a96 375 // sqrt(accX*accX + accY*accY + accZ*accZ), sqrt(accXAvg*accXAvg + accYAvg*accYAvg + accZAvg*accZAvg), state);
DGonz 23:81a709626a96 376 // pcSerial.printf("%f, %f, %f, \n", roll, pitch, yaw);
DGonz 23:81a709626a96 377 pcSerial.printf("%f, ",t);
DGonz 23:81a709626a96 378 pcSerial.printf("%f, %f, ", roll, pitch);
DGonz 23:81a709626a96 379 pcSerial.printf("%f, %f, ", rollDot, pitchDot);
DGonz 23:81a709626a96 380 pcSerial.printf("%f, %f, %f, %f ", odrvCmds[0], odrvCmds[1], odrvCmds[2], odrvCmds[3]);
DGonz 23:81a709626a96 381 pcSerial.printf("%f, %i \n", sqrt(accX*accX + accY*accY + accZ*accZ), state);
DGonz 23:81a709626a96 382
DGonz 23:81a709626a96 383 // pcSerial.printf("%f, %f, %f, %f, %f, %f, %f, %f, %i\n",
DGonz 23:81a709626a96 384 // accX, accY, accZ, accXAvg, accYAvg, accZAvg,
DGonz 23:81a709626a96 385 // sqrt(accX*accX + accY*accY + accZ*accZ), sqrt(accXAvg*accXAvg + accYAvg*accYAvg + accZAvg*accZAvg), state);
DGonz 20:053dbb6a7ef5 386 }
DGonz 23:81a709626a96 387 pitchAvg=0;
DGonz 23:81a709626a96 388 rollAvg=0;
DGonz 23:81a709626a96 389 yawAvg=0;
DGonz 23:81a709626a96 390 rollDotAvg=0;
DGonz 23:81a709626a96 391 pitchDotAvg=0;
DGonz 23:81a709626a96 392 yawDotAvg=0;
DGonz 23:81a709626a96 393 accXAvg=0;
DGonz 23:81a709626a96 394 accYAvg=0;
DGonz 23:81a709626a96 395 accZAvg=0;
DGonz 20:053dbb6a7ef5 396 loopCounter = 0;
DGonz 20:053dbb6a7ef5 397 }
DGonz 20:053dbb6a7ef5 398 }